From 2f6f874542b4b5d2b99d273e9189a7e0fa80530a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Aug 2016 19:38:28 -0400 Subject: [PATCH] 0.11.10: Updated LaserScanInfo interface. Updated NodeData msg with occupancy grids and laser scan local transform. MapsManager is now using directly the occupancy grids saved in nodes for 3D cloud, octomap and grid map. --- CMakeLists.txt | 2 +- include/rtabmap_ros/CoreWrapper.h | 11 +- include/rtabmap_ros/MapsManager.h | 53 +- msg/NodeData.msg | 8 + package.xml | 2 +- src/CoreNode.cpp | 5 +- src/CoreWrapper.cpp | 313 +++++---- src/GuiWrapper.cpp | 148 +++- src/MapAssemblerNode.cpp | 3 - src/MapsManager.cpp | 1037 +++++++++++------------------ src/MsgConversion.cpp | 83 ++- src/nodelets/icp_odometry.cpp | 22 +- src/nodelets/rgbdicp_odometry.cpp | 25 +- 13 files changed, 800 insertions(+), 912 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c6614ec..76b532dc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(rviz) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.11.9 REQUIRED) +find_package(RTABMap 0.11.10 REQUIRED) find_package(OpenCV REQUIRED) diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index a4421773..c6d6e7d5 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -77,6 +77,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include typedef actionlib::SimpleActionClient MoveBaseClient; +namespace rtabmap { +class StereoDense; +} + class CoreWrapper { public: @@ -226,7 +230,8 @@ private: bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res); + bool getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res); + bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&); @@ -239,7 +244,7 @@ private: bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); #endif - rtabmap::ParametersMap loadParameters(const std::string & configFile); + void loadParameters(const std::string & configFile, rtabmap::ParametersMap & parameters); void saveParameters(const std::string & configFile); void publishLoop(double tfDelay, double tfTolerance); @@ -489,6 +494,7 @@ private: ros::ServiceServer setLogErrorSrv_; ros::ServiceServer getMapDataSrv_; ros::ServiceServer getProjMapSrv_; + ros::ServiceServer getMapSrv_; ros::ServiceServer getGridMapSrv_; ros::ServiceServer publishMapDataSrv_; ros::ServiceServer setGoalSrv_; @@ -504,6 +510,7 @@ private: boost::thread* transformThread_; + bool stereoToDepth_; float rate_; bool createIntermediateNodes_; ros::Time time_; diff --git a/include/rtabmap_ros/MapsManager.h b/include/rtabmap_ros/MapsManager.h index 6c1cb72e..f665bb69 100644 --- a/include/rtabmap_ros/MapsManager.h +++ b/include/rtabmap_ros/MapsManager.h @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define MAPSMANAGER_H_ #include +#include #include #include #include @@ -37,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { class OctoMap; class Memory; +class OccupancyGrid; } // namespace rtabmap @@ -46,6 +48,8 @@ public: virtual ~MapsManager(); void clear(); bool hasSubscribers() const; + void backwardCompatibilityParameters(rtabmap::ParametersMap & parameters) const; + void setParameters(const rtabmap::ParametersMap & parameters); std::map getFilteredPoses( const std::map & poses); @@ -53,10 +57,7 @@ public: std::map updateMapCaches( const std::map & poses, const rtabmap::Memory * memory, - bool updateCloud, - bool updateProj, bool updateGrid, - bool updateScan, bool updateOctomap, const std::map & signatures = std::map()); @@ -65,12 +66,6 @@ public: const ros::Time & stamp, const std::string & mapFrameId); - cv::Mat generateProjMap( - const std::map & filteredPoses, - float & xMin, - float & yMin, - float & gridCellSize); - cv::Mat generateGridMap( const std::map & filteredPoses, float & xMin, @@ -81,37 +76,20 @@ public: private: // mapping stuff - int cloudDecimation_; - double cloudMaxDepth_; - double cloudMinDepth_; - double cloudVoxelSize_; - double cloudFloorCullingHeight_; - double cloudCeilingCullingHeight_; bool cloudOutputVoxelized_; - bool cloudFrustumCulling_; - double cloudNoiseFilteringRadius_; - int cloudNoiseFilteringMinNeighbors_; - int scanDecimation_; - double scanVoxelSize_; - bool scanOutputVoxelized_; - double projMaxGroundAngle_; - int projMinClusterSize_; - double projMaxObstaclesHeight_; - double projMaxGroundHeight_; - bool projDetectFlatObstacles_; - bool projMapFrame_; double gridCellSize_; + bool gridIncremental_; double gridSize_; bool gridEroded_; double footprintRadius_; - bool gridUnknownSpaceFilled_; - double gridMaxUnknownSpaceFilledRange_; double mapFilterRadius_; double mapFilterAngle_; bool mapCacheCleanup_; bool negativePosesIgnored_; ros::Publisher cloudMapPub_; + ros::Publisher cloudGroundPub_; + ros::Publisher cloudObstaclesPub_; ros::Publisher projMapPub_; ros::Publisher gridMapPub_; ros::Publisher scanMapPub_; @@ -121,15 +99,22 @@ private: ros::Publisher octoMapEmptySpace_; ros::Publisher octoMapProj_; - std::map::Ptr > clouds_; - std::map::Ptr > scans_; - std::map > cameraModels_; - std::map > projMaps_; // + std::map assembledGroundPoses_; + std::map assembledObstaclePoses_; + pcl::PointCloud::Ptr assembledObstacles_; + pcl::PointCloud::Ptr assembledGround_; + + std::map gridPoses_; + cv::Mat gridMap_; std::map > gridMaps_; // + std::map gridMapsViewpoints_; + + rtabmap::OccupancyGrid * occupancyGrid_; rtabmap::OctoMap * octomap_; int octomapTreeDepth_; - bool octomapGroundIsObstacle_; + + rtabmap::ParametersMap parameters_; }; #endif /* MAPSMANAGER_H_ */ diff --git a/msg/NodeData.msg b/msg/NodeData.msg index f9ce1d0d..ed560c50 100644 --- a/msg/NodeData.msg +++ b/msg/NodeData.msg @@ -33,11 +33,19 @@ geometry_msgs/Transform[] localTransform uint8[] laserScan int32 laserScanMaxPts float32 laserScanMaxRange +geometry_msgs/Transform laserScanLocalTransform # compressed user data # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" uint8[] userData +# compressed occupancy grid +# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +uint8[] grid_ground +uint8[] grid_obstacles +float32 grid_cell_size +Point3f grid_view_point + # std::multimap # std::multimap int32[] wordIds diff --git a/package.xml b/package.xml index c815632b..6eab9a07 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.11.9 + 0.11.10 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/src/CoreNode.cpp b/src/CoreNode.cpp index df608f12..eacf6972 100644 --- a/src/CoreNode.cpp +++ b/src/CoreNode.cpp @@ -51,9 +51,8 @@ int main(int argc, char** argv) else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0) { rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); - uInsert(parameters, - std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(), - UDirectory::homeDir()+"/.ros")); // change default to ~/.ros + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros if(strcmp(argv[i], "--params") == 0) { diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index c18ca3cb..2cb8f85a 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -124,6 +125,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) stereoApproxTFSync_(0), stereoExactTFSync_(0), transformThread_(0), + stereoToDepth_(false), rate_(Parameters::defaultRtabmapDetectionRate()), createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()), time_(ros::Time::now()), @@ -208,6 +210,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_); pnh.param("flip_scan", flipScan_, flipScan_); + pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_); if(!tfPrefix.empty()) { @@ -285,12 +288,15 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) databasePath_ = UDirectory::currentDir(true) + databasePath_; } + ParametersMap allParameters = Parameters::getDefaultParameters(); + uInsert(allParameters, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS + uInsert(allParameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros + // load parameters - parameters_ = loadParameters(configPath_); + loadParameters(configPath_, parameters_); // update parameters with user input parameters (private) - uInsert(parameters_, std::make_pair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros - for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) + for(ParametersMap::iterator iter=allParameters.begin(); iter!=allParameters.end(); ++iter) { std::string vStr; bool vBool; @@ -299,31 +305,31 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) if(pnh.getParam(iter->first, vStr)) { ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); - iter->second = vStr; if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0) { - iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir()); + vStr = uReplaceChar(vStr, '~', UDirectory::homeDir()); } else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0) { - iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir()); + vStr = uReplaceChar(vStr, '~', UDirectory::homeDir()); } + uInsert(parameters_, ParametersPair(iter->first, vStr)); } else if(pnh.getParam(iter->first, vBool)) { ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); - iter->second = uBool2Str(vBool); + uInsert(parameters_, ParametersPair(iter->first, uBool2Str(vBool))); } else if(pnh.getParam(iter->first, vDouble)) { ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); - iter->second = uNumber2Str(vDouble); + uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vDouble))); } else if(pnh.getParam(iter->first, vInt)) { ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); - iter->second = uNumber2Str(vInt); + uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vInt))); } } @@ -345,7 +351,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) if(iter->second.first) { // can be migrated - parameters_.at(iter->second.second)= vStr; + uInsert(parameters_, ParametersPair(iter->second.second, vStr)); ROS_WARN("Rtabmap: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); } @@ -365,6 +371,25 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) } } + // Backward compatibility (MapsManager) + mapsManager_.backwardCompatibilityParameters(parameters_); + + if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end()) + { + ROS_WARN("Setting \"%s\" parameter to false (default true) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is " + "true. The occupancy grid map will be constructed from " + "laser scans. To get occupancy grid map from cloud projection, set \"%s\" " + "to true. To suppress this warning, " + "add ", + Parameters::kGridFromDepth().c_str(), + Parameters::kGridFromDepth().c_str(), + Parameters::kGridFromDepth().c_str()); + parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false")); + } + + // Add all other parameters (not copied if already exists) + parameters_.insert(allParameters.begin(), allParameters.end()); + // set public parameters nh.setParam("is_rtabmap_paused", paused_); for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) @@ -391,9 +416,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) if(!subscribeDepth && !subscribeStereo) { ROS_WARN("ROS param subscribe_depth and subscribe_stereo are false, but RTAB-Map " - "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth or subscribe_stereo " - "to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure " - "detection on images-only."); + "parameter \"%s\" is true! Please set subscribe_depth or subscribe_stereo " + "to true to use rtabmap node for RGB-D SLAM, or set \"%s\" to false for loop closure " + "detection on images-only.", Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str()); } } @@ -419,6 +444,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) ROS_INFO("rtabmap: database_path parameter not set, the map will not be saved."); } + mapsManager_.setParameters(parameters_); + if(subscribeStereo) + { + ROS_INFO("rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false"); + } + // Init RTAB-Map rtabmap_.init(parameters_, databasePath_); @@ -436,7 +467,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this); setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this); setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this); - getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this); + getMapDataSrv_ = nh.advertiseService("get_map_data", &CoreWrapper::getMapDataCallback, this); + getMapSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this); getGridMapSrv_ = nh.advertiseService("get_grid_map", &CoreWrapper::getGridMapCallback, this); getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this); publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this); @@ -550,9 +582,8 @@ CoreWrapper::~CoreWrapper() printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str()); } -ParametersMap CoreWrapper::loadParameters(const std::string & configFile) +void CoreWrapper::loadParameters(const std::string & configFile, ParametersMap & parameters) { - ParametersMap parameters = Parameters::getDefaultParameters(); if(!configFile.empty()) { ROS_INFO("Loading parameters from %s", configFile.c_str()); @@ -562,9 +593,6 @@ ParametersMap CoreWrapper::loadParameters(const std::string & configFile) } Parameters::readINI(configFile.c_str(), parameters); } - // otherwise take default parameters - - return parameters; } void CoreWrapper::saveParameters(const std::string & configFile) @@ -993,12 +1021,14 @@ void CoreWrapper::commonDepthCallback( } cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, + scanLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, - scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull()) + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)); + if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec()); return; @@ -1007,7 +1037,7 @@ void CoreWrapper::commonDepthCallback( //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -1024,8 +1054,7 @@ void CoreWrapper::commonDepthCallback( } else { - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } @@ -1051,13 +1080,12 @@ void CoreWrapper::commonDepthCallback( } // sync with odometry stamp - Transform localScanTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); - if(localScanTransform.isNull()) + scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); + if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec()); return; } - Transform laserOdomT = localScanTransform; if(lastPoseStamp_ != scan3dMsg->header.stamp) { if(!odomT.isNull()) @@ -1070,7 +1098,7 @@ void CoreWrapper::commonDepthCallback( } else { - laserOdomT = odomT.inverse() * sensorT * localScanTransform; + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } @@ -1080,10 +1108,6 @@ void CoreWrapper::commonDepthCallback( { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*scan3dMsg, *pclScan); - if(!laserOdomT.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, laserOdomT); - } scan = util3d::laserScanFromPointCloud(*pclScan); } else @@ -1091,11 +1115,6 @@ void CoreWrapper::commonDepthCallback( pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*scan3dMsg, *pclScan); - if(!laserOdomT.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, laserOdomT); - } - if(scanCloudNormalK_ > 0) { //compute normals @@ -1126,8 +1145,10 @@ void CoreWrapper::commonDepthCallback( } SensorData data(scan, - scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0), - scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), + LaserScanInfo( + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0), + scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), + scanLocalTransform), rgb, depth, cameraModels, @@ -1167,6 +1188,77 @@ void CoreWrapper::commonStereoCallback( return; } + cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage; + if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8"); + } + else + { + ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8"); + } + ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8"); + + Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp); + if(localTransform.isNull()) + { + return; + } + + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); + + if(stereoModel.baseline() > 10.0) + { + static bool shown = false; + if(!shown) + { + ROS_WARN("Detected baseline (%f m) is quite large! Is your " + "right camera_info P(0,3) correctly set? Note that " + "baseline=-P(0,3)/P(0,0). This warning is printed only once.", + stereoModel.baseline()); + shown = true; + } + } + + if(stereoToDepth_) + { + // cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters + cv::Mat disparity = util2d::disparityFromStereoImages(ptrLeftImage->image, ptrRightImage->image, parameters_); + if(disparity.empty()) + { + ROS_ERROR("Could not compute disparity image (\"stereo_to_depth\" is true)!"); + return; + } + cv::Mat depth = util2d::depthFromDisparity( + disparity, + stereoModel.left().fx(), + stereoModel.baseline()); + + if(depth.empty()) + { + ROS_ERROR("Could not compute depth image (\"stereo_to_depth\" is true)!"); + return; + } + UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); + + // move to common depth callback + cv_bridge::CvImage imgDepth; + if(depth.type() == CV_16UC1) + { + imgDepth.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + } + else // CV_32FC1 + { + imgDepth.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + } + imgDepth.image = depth; + sensor_msgs::ImagePtr depthMsg = imgDepth.toImageMsg(); + depthMsg->header = leftImageMsg->header; + + commonDepthCallback(odomFrameId, leftImageMsg, depthMsg, leftCamInfoMsg, scan2dMsg, scan3dMsg); + } + //for sync transform Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_); if(odomT.isNull() && !odomFrameId_.empty()) @@ -1175,12 +1267,6 @@ void CoreWrapper::commonStereoCallback( odomFrameId.c_str(), frameId_.c_str()); } - Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp); - if(localTransform.isNull()) - { - return; - } - // sync with odometry stamp if(lastPoseStamp_ != leftImageMsg->header.stamp) { @@ -1196,12 +1282,14 @@ void CoreWrapper::commonStereoCallback( } cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, + scanLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, - scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull()) + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)); + if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec()); return; @@ -1211,7 +1299,7 @@ void CoreWrapper::commonStereoCallback( sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; //projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfBuffer_); - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -1225,9 +1313,7 @@ void CoreWrapper::commonStereoCallback( { return; } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); - + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } @@ -1252,13 +1338,12 @@ void CoreWrapper::commonStereoCallback( } // sync with odometry stamp - Transform localScanTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); - if(localScanTransform.isNull()) + scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); + if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec()); return; } - Transform laserOdomT = localScanTransform; if(lastPoseStamp_ != scan3dMsg->header.stamp) { if(!odomT.isNull()) @@ -1271,7 +1356,7 @@ void CoreWrapper::commonStereoCallback( } else { - laserOdomT = odomT.inverse() * sensorT * localScanTransform; + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } @@ -1282,11 +1367,6 @@ void CoreWrapper::commonStereoCallback( pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*scan3dMsg, *pclScan); - if(!laserOdomT.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, laserOdomT); - } - scan = util3d::laserScanFromPointCloud(*pclScan); } else @@ -1294,11 +1374,6 @@ void CoreWrapper::commonStereoCallback( pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*scan3dMsg, *pclScan); - if(!laserOdomT.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, laserOdomT); - } - if(scanCloudNormalK_ > 0) { //compute normals @@ -1314,33 +1389,6 @@ void CoreWrapper::commonStereoCallback( } } - cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage; - if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8"); - } - else - { - ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8"); - } - ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8"); - - rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); - - if(stereoModel.baseline() > 10.0) - { - static bool shown = false; - if(!shown) - { - ROS_WARN("Detected baseline (%f m) is quite large! Is your " - "right camera_info P(0,3) correctly set? Note that " - "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - stereoModel.baseline()); - shown = true; - } - } - ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp: scan3dMsg.get() != 0?scan3dMsg->header.stamp: leftImageMsg->header.stamp; @@ -1352,8 +1400,10 @@ void CoreWrapper::commonStereoCallback( } SensorData data(scan, - scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0, - scan2dMsg.get() != 0?scan2dMsg->range_max:0, + LaserScanInfo( + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0, + scan2dMsg.get() != 0?scan2dMsg->range_max:0, + scanLocalTransform), ptrLeftImage->image, ptrRightImage->image, stereoModel, @@ -1621,9 +1671,6 @@ void CoreWrapper::process( rtabmap_.getMemory(), false, false, - false, - false, - false, tmpSignature); timeUpdateMaps = timer.ticks(); @@ -1916,6 +1963,7 @@ bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp ROS_INFO("RTAB-Map rate detection = %f Hz", rate_); } rtabmap_.parseParameters(parameters_); + mapsManager_.setParameters(parameters_); return true; } @@ -2039,7 +2087,7 @@ bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon return true; } -bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res) +bool CoreWrapper::getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res) { ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...", req.global?"true":"false", @@ -2083,65 +2131,41 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros: bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) { - std::map filteredPoses; - filteredPoses = mapsManager_.updateMapCaches( - rtabmap_.getLocalOptimizedPoses(), - rtabmap_.getMemory(), - false, - true, - false, - false, - false); - if(filteredPoses.size()) + if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() && + !uStr2Bool(parameters_.at(Parameters::kGridFromDepth()))) { - // create the projection map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize); - - if(!pixels.empty()) - { - //init - res.map.info.resolution = gridCellSize; - res.map.info.origin.position.x = 0.0; - res.map.info.origin.position.y = 0.0; - res.map.info.origin.position.z = 0.0; - res.map.info.origin.orientation.x = 0.0; - res.map.info.origin.orientation.y = 0.0; - res.map.info.origin.orientation.z = 0.0; - res.map.info.origin.orientation.w = 1.0; - - res.map.info.width = pixels.cols; - res.map.info.height = pixels.rows; - res.map.info.origin.position.x = xMin; - res.map.info.origin.position.y = yMin; - res.map.data.resize(res.map.info.width * res.map.info.height); - - memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height); - - res.map.header.frame_id = mapFrameId_; - res.map.header.stamp = ros::Time::now(); - return true; - } + ROS_WARN("/get_proj_map service is deprecated! Call /get_grid_map service " + "instead with . " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + Parameters::kGridFromDepth().c_str()); } - return false; + else + { + ROS_WARN("/get_proj_map service is deprecated! Call /get_grid_map service instead."); + } + return getGridMapCallback(req, res); } bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) +{ + ROS_WARN("/get_grid_map service is deprecated! Call /get_map service instead."); + return getMapCallback(req, res); +} + +bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) { std::map filteredPoses; filteredPoses = mapsManager_.updateMapCaches( rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory(), - false, - false, true, - false, false); if(filteredPoses.size()) { // create the grid map float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize); + cv::Mat pixels = mapsManager_.generateGridMap(filteredPoses, xMin, yMin, gridCellSize); if(!pixels.empty()) { @@ -2247,9 +2271,6 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab rtabmap_.getMemory(), false, false, - false, - false, - false, signatures); } else @@ -2715,7 +2736,7 @@ bool CoreWrapper::octomapBinaryCallback( res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true); const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map); @@ -2731,7 +2752,7 @@ bool CoreWrapper::octomapFullCallback( res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true); const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map); diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index a3f51128..51f1919c 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -407,9 +407,9 @@ void GuiWrapper::handleEvent(UEvent * anEvent) getMapSrv.request.global = cmdEvent->value1().toBool(); getMapSrv.request.optimized = cmdEvent->value2().toBool(); getMapSrv.request.graphOnly = cmdEvent->value3().toBool(); - if(!ros::service::call("get_map", getMapSrv)) + if(!ros::service::call("get_map_data", getMapSrv)) { - ROS_WARN("Can't call \"get_map\" service"); + ROS_WARN("Can't call \"get_map_data\" service"); this->post(new RtabmapEvent3DMap(1)); // service error } else @@ -588,6 +588,7 @@ void GuiWrapper::commonDepthCallback( cv::Mat depth; std::vector cameraModels; cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); rtabmap::OdometryInfo info; bool ignoreData = false; @@ -693,15 +694,20 @@ void GuiWrapper::commonDepthCallback( if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) + scanLocalTransform = getTransform( + frameId_, + scan2dMsg->header.frame_id, + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)); + if(scanLocalTransform.isNull()) { + ROS_ERROR("TF of received scan at time %fs is not set, aborting rtabmapviz update.", scan2dMsg->header.stamp.toSec()); return; } //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -715,18 +721,62 @@ void GuiWrapper::commonDepthCallback( { return; } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); - + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } scan = util3d::laserScanFromPointCloud(*pclScan); } else if(scan3dMsg.get() != 0) { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*scan3dMsg, *pclScan); - scan = util3d::laserScanFromPointCloud(*pclScan); + bool containNormals = false; + for(unsigned int i=0; ifields.size(); ++i) + { + if(scan3dMsg->fields[i].name.compare("normal_x") == 0) + { + containNormals = true; + break; + } + } + + // sync with odometry stamp + scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); + if(scanLocalTransform.isNull()) + { + ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec()); + return; + } + if(odomHeader.stamp != scan3dMsg->header.stamp) + { + if(!odomT.isNull()) + { + Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan3dMsg->header.stamp); + if(sensorT.isNull()) + { + ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry " + "stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomHeader.stamp.toSec()); + } + else + { + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; + } + + } + } + + if(containNormals) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + scan = util3d::laserScanFromPointCloud(*pclScan); + } + else + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + scan = util3d::laserScanFromPointCloud(*pclScan); + } } if(odomInfoMsg.get()) @@ -749,8 +799,10 @@ void GuiWrapper::commonDepthCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, - scan2dMsg.get()?(int)scan2dMsg->range_max:0, + LaserScanInfo( + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, + scanLocalTransform), rgb, depth, cameraModels, @@ -854,6 +906,7 @@ void GuiWrapper::commonStereoCallback( cv::Mat left; cv::Mat right; cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); rtabmap::StereoCameraModel stereoModel; rtabmap::OdometryInfo info; bool ignoreData = false; @@ -898,15 +951,20 @@ void GuiWrapper::commonStereoCallback( if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) + scanLocalTransform = getTransform( + frameId_, + scan2dMsg->header.frame_id, + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)); + if(scanLocalTransform.isNull()) { + ROS_ERROR("TF of received scan at time %fs is not set, aborting rtabmapviz update.", scan2dMsg->header.stamp.toSec()); return; } //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -920,18 +978,62 @@ void GuiWrapper::commonStereoCallback( { return; } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); - + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } scan = util3d::laserScan2dFromPointCloud(*pclScan); } else if(scan3dMsg.get() != 0) { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*scan3dMsg, *pclScan); - scan = util3d::laserScanFromPointCloud(*pclScan); + bool containNormals = false; + for(unsigned int i=0; ifields.size(); ++i) + { + if(scan3dMsg->fields[i].name.compare("normal_x") == 0) + { + containNormals = true; + break; + } + } + + // sync with odometry stamp + scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); + if(scanLocalTransform.isNull()) + { + ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec()); + return; + } + if(odomHeader.stamp != scan3dMsg->header.stamp) + { + if(!odomT.isNull()) + { + Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan3dMsg->header.stamp); + if(sensorT.isNull()) + { + ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry " + "stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomHeader.stamp.toSec()); + } + else + { + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; + } + + } + } + + if(containNormals) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + scan = util3d::laserScanFromPointCloud(*pclScan); + } + else + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + scan = util3d::laserScanFromPointCloud(*pclScan); + } } if(odomInfoMsg.get()) @@ -954,8 +1056,10 @@ void GuiWrapper::commonStereoCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, - scan2dMsg.get()?(int)scan2dMsg->range_max:0, + LaserScanInfo( + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, + scanLocalTransform), left, right, stereoModel, diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index 8e7c295d..ce63ff4a 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -99,9 +99,6 @@ public: 0, false, false, - false, - false, - false, nodes_); mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id); diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 32f2cb6c..3c99afae 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -47,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP #include +#include #include #endif #endif @@ -54,96 +56,38 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace rtabmap; MapsManager::MapsManager(bool usePublicNamespace) : - cloudDecimation_(4), - cloudMaxDepth_(4.0), // meters - cloudMinDepth_(0.0), // meters - cloudVoxelSize_(0.05), // meters - cloudFloorCullingHeight_(0.0), - cloudCeilingCullingHeight_(0.0), - cloudOutputVoxelized_(false), - cloudFrustumCulling_(false), - cloudNoiseFilteringRadius_(0.0), - cloudNoiseFilteringMinNeighbors_(5), - scanDecimation_(0), - scanVoxelSize_(0.0), - scanOutputVoxelized_(false), - projMaxGroundAngle_(45.0), // degrees - projMinClusterSize_(20), - projMaxObstaclesHeight_(2.0), // meters (<=0 disabled) - projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true) - projDetectFlatObstacles_(false), - projMapFrame_(false), + cloudOutputVoxelized_(true), gridCellSize_(0.05), // meters + gridIncremental_(false), gridSize_(0), // meters gridEroded_(false), footprintRadius_(0.0), - gridUnknownSpaceFilled_(false), - gridMaxUnknownSpaceFilledRange_(6.0), mapFilterRadius_(0.0), mapFilterAngle_(30.0), // degrees mapCacheCleanup_(true), - negativePosesIgnored_(false), + negativePosesIgnored_(true), + assembledObstacles_(new pcl::PointCloud), + assembledGround_(new pcl::PointCloud), + occupancyGrid_(new OccupancyGrid), octomap_(0), - octomapTreeDepth_(16), - octomapGroundIsObstacle_(false) + octomapTreeDepth_(16) { ros::NodeHandle nh; ros::NodeHandle pnh("~"); - // cloud map stuff - pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_); - pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_); - pnh.param("cloud_min_depth", cloudMinDepth_, cloudMinDepth_); - pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_); - pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_); - pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_); - if(cloudFloorCullingHeight_ > 0 && - cloudCeilingCullingHeight_ > 0 && - cloudCeilingCullingHeight_ < cloudFloorCullingHeight_) - { - ROS_WARN("\"cloud_floor_culling_height\" should be lower than \"cloud_ceiling_culling_height\", setting \"cloud_ceiling_culling_height\" to 0 (disabled)."); - cloudCeilingCullingHeight_ = 0; - } - pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_); - pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_); - pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_); - pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_); - - // scan map stuff - pnh.param("scan_decimation", scanDecimation_, scanDecimation_); - pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); - pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_); - - //projection map stuff - pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_); - pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_); - if(pnh.hasParam("proj_max_height") && !pnh.hasParam("proj_max_obstacles_height")) - { - ROS_WARN("Parameter \"proj_max_height\" has been renamed " - "to \"proj_max_obstacles_height\"! Your value is still copied to " - "corresponding parameter."); - pnh.param("proj_max_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_); - } - else - { - pnh.param("proj_max_obstacles_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_); - } - pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_); - pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_); - pnh.param("proj_map_frame", projMapFrame_, projMapFrame_); - // common grid map stuff pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m if(gridCellSize_ <= 0) { ROS_FATAL("\"grid_cell_size\" (%f) should be greater than 0!", gridCellSize_); } + occupancyGrid_->setCellSize(gridCellSize_); + + pnh.param("grid_incremental", gridIncremental_, gridIncremental_); // m pnh.param("grid_size", gridSize_, gridSize_); // m pnh.param("grid_eroded", gridEroded_, gridEroded_); pnh.param("grid_footprint_radius", footprintRadius_, footprintRadius_); - pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_); - pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_); // common map stuff pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_); @@ -151,6 +95,17 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_); pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_); + if(pnh.hasParam("scan_output_voxelized")) + { + ROS_WARN("Parameter \"scan_output_voxelized\" has been " + "removed. Use \"cloud_output_voxelized\" instead."); + if(!pnh.hasParam("cloud_output_voxelized")) + { + pnh.getParam("scan_output_voxelized", cloudOutputVoxelized_); + } + } + pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_); + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP octomap_ = new OctoMap(gridCellSize_); @@ -165,7 +120,6 @@ MapsManager::MapsManager(bool usePublicNamespace) : ROS_WARN("octomap_tree_depth cannot be negative, set to 16 instead"); octomapTreeDepth_ = 16; } - pnh.param("octomap_ground_is_obstacle", octomapGroundIsObstacle_, octomapGroundIsObstacle_); #endif #endif @@ -176,43 +130,32 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("latch", latch, latch); // mapping topics - if(usePublicNamespace) - { - cloudMapPub_ = nh.advertise("cloud_map", 1, latch); - projMapPub_ = nh.advertise("proj_map", 1, latch); - gridMapPub_ = nh.advertise("grid_map", 1, latch); - scanMapPub_ = nh.advertise("scan_map", 1, latch); + ros::NodeHandle nht(usePublicNamespace?"":"~"); + gridMapPub_ = nht.advertise("grid_map", 1, latch); + cloudMapPub_ = nht.advertise("cloud_map", 1, latch); + cloudObstaclesPub_ = nht.advertise("cloud_obstacles", 1, latch); + cloudGroundPub_ = nht.advertise("cloud_ground", 1, latch); + + // deprecated + projMapPub_ = nht.advertise("proj_map", 1, latch); + scanMapPub_ = nht.advertise("scan_map", 1, latch); + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP - octoMapPubBin_ = nh.advertise("octomap_binary", 1, latch); - octoMapPubFull_ = nh.advertise("octomap_full", 1, latch); - octoMapCloud_ = nh.advertise("octomap_cloud", 1, latch); - octoMapEmptySpace_ = nh.advertise("octomap_empty_space", 1, latch); - octoMapProj_ = nh.advertise("octomap_proj", 1, latch); + octoMapPubBin_ = nht.advertise("octomap_binary", 1, latch); + octoMapPubFull_ = nht.advertise("octomap_full", 1, latch); + octoMapCloud_ = nht.advertise("octomap_occupied_space", 1, latch); + octoMapEmptySpace_ = nht.advertise("octomap_empty_space", 1, latch); + octoMapProj_ = nht.advertise("octomap_grid", 1, latch); #endif #endif - } - else - { - cloudMapPub_ = pnh.advertise("cloud_map", 1, latch); - projMapPub_ = pnh.advertise("proj_map", 1, latch); - gridMapPub_ = pnh.advertise("grid_map", 1, latch); - scanMapPub_ = pnh.advertise("scan_map", 1, latch); -#ifdef WITH_OCTOMAP_ROS -#ifdef RTABMAP_OCTOMAP - octoMapPubBin_ = pnh.advertise("octomap_binary", 1, latch); - octoMapPubFull_ = pnh.advertise("octomap_full", 1, latch); - octoMapCloud_ = pnh.advertise("octomap_cloud", 1, latch); - octoMapEmptySpace_ = pnh.advertise("octomap_cloud_ground", 1, latch); - octoMapProj_ = pnh.advertise("octomap_proj", 1, latch); -#endif -#endif - } } MapsManager::~MapsManager() { clear(); + delete occupancyGrid_; + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP if(octomap_) @@ -224,12 +167,106 @@ MapsManager::~MapsManager() { #endif } +void parameterMoved( + ros::NodeHandle & nh, + const std::string & rosName, + const std::string & parameterName, + ParametersMap & parameters) +{ + if(nh.hasParam(rosName)) + { + ParametersMap::const_iterator iter = Parameters::getDefaultParameters().find(parameterName); + if(iter != Parameters::getDefaultParameters().end()) + { + ROS_WARN("Parameter \"%s\" has moved from " + "rtabmap_ros to rtabmap library. Use " + "parameter \"%s\" instead. The value is still " + "copied to new parameter name.", + rosName.c_str(), + parameterName.c_str()); + std::string type = Parameters::getType(parameterName); + if(type.compare("float") || type.compare("double")) + { + double v = uStr2Double(iter->second); + nh.getParam(rosName, v); + parameters.insert(ParametersPair(parameterName, uNumber2Str(v))); + } + else if(type.compare("int") || type.compare("unsigned int")) + { + int v = uStr2Int(iter->second); + nh.getParam(rosName, v); + parameters.insert(ParametersPair(parameterName, uNumber2Str(v))); + } + else + { + ROS_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str()); + } + } + else + { + ROS_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str()); + } + } +} + +void MapsManager::backwardCompatibilityParameters(ParametersMap & parameters) const +{ + ros::NodeHandle pnh("~"); + + // removed + if(pnh.hasParam("cloud_frustum_culling")) + { + ROS_WARN("Parameter \"cloud_frustum_culling\" has been removed. OctoMap topics " + "already do it. You can remove it from your launch file."); + } + + // moved + parameterMoved(pnh, "cloud_decimation", Parameters::kGridDepthDecimation(), parameters); + parameterMoved(pnh, "cloud_max_depth", Parameters::kGridDepthMax(), parameters); + parameterMoved(pnh, "cloud_min_depth", Parameters::kGridDepthMin(), parameters); + parameterMoved(pnh, "cloud_voxel_size", Parameters::kGridCellSize(), parameters); + parameterMoved(pnh, "cloud_floor_culling_height", Parameters::kGridMaxGroundHeight(), parameters); + parameterMoved(pnh, "cloud_ceiling_culling_height", Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "cloud_noise_filtering_radius", Parameters::kGridNoiseFilteringRadius(), parameters); + parameterMoved(pnh, "cloud_noise_filtering_min_neighbors", Parameters::kGridNoiseFilteringMinNeighbors(), parameters); + parameterMoved(pnh, "scan_decimation", Parameters::kGridScanDecimation(), parameters); + parameterMoved(pnh, "scan_voxel_size", Parameters::kGridCellSize(), parameters); + parameterMoved(pnh, "proj_max_ground_angle", Parameters::kGridMaxGroundAngle(), parameters); + parameterMoved(pnh, "proj_min_cluster_size", Parameters::kGridMinClusterSize(), parameters); + parameterMoved(pnh, "proj_max_height", Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "proj_max_obstacles_height", Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "proj_max_ground_height", Parameters::kGridMaxGroundHeight(), parameters); + + parameterMoved(pnh, "proj_detect_flat_obstacles", Parameters::kGridFlatObstacleDetected(), parameters); + parameterMoved(pnh, "proj_map_frame", Parameters::kGridMapFrameProjection(), parameters); + parameterMoved(pnh, "grid_unknown_space_filled", Parameters::kGridScan2dUnknownSpaceFilled(), parameters); + parameterMoved(pnh, "grid_unknown_space_filled_max_range", Parameters::kGridScan2dMaxFilledRange(), parameters); + +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + parameterMoved(pnh, "octomap_ground_is_obstacle", Parameters::kGrid3DGroundIsObstacle(), parameters); +#endif +#endif +} + +void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) +{ + parameters_ = parameters; + + // don't use grid cell size from parameters as we use grid_cell_size ros param + uInsert(parameters_, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(gridCellSize_))); + occupancyGrid_->parseParameters(parameters_); +} + void MapsManager::clear() { - clouds_.clear(); - cameraModels_.clear(); - projMaps_.clear(); gridMaps_.clear(); + gridMapsViewpoints_.clear(); + assembledGround_->clear(); + assembledObstacles_->clear(); + assembledGroundPoses_.clear(); + assembledObstaclePoses_.clear(); + occupancyGrid_->clear(); #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP octomap_->clear(); @@ -240,6 +277,8 @@ void MapsManager::clear() bool MapsManager::hasSubscribers() const { return cloudMapPub_.getNumSubscribers() != 0 || + cloudObstaclesPub_.getNumSubscribers() != 0 || + cloudGroundPub_.getNumSubscribers() != 0 || projMapPub_.getNumSubscribers() != 0 || gridMapPub_.getNumSubscribers() != 0 || scanMapPub_.getNumSubscribers() != 0 || @@ -264,20 +303,14 @@ std::map MapsManager::getFilteredPoses(const std::map MapsManager::updateMapCaches( const std::map & poses, const rtabmap::Memory * memory, - bool updateCloud, - bool updateProj, bool updateGrid, - bool updateScan, bool updateOctomap, const std::map & signatures) { - if(!updateCloud && !updateProj && !updateGrid && !updateScan && !updateOctomap) + if(!updateGrid && !updateOctomap) { // all false, udpate only those where we have subscribers - updateCloud = cloudMapPub_.getNumSubscribers() != 0; - updateProj = projMapPub_.getNumSubscribers() != 0; - updateGrid = gridMapPub_.getNumSubscribers() != 0; - updateScan = scanMapPub_.getNumSubscribers() != 0; + updateGrid = this->hasSubscribers(); updateOctomap = octoMapPubBin_.getNumSubscribers() != 0 || octoMapPubFull_.getNumSubscribers() != 0 || @@ -305,7 +338,7 @@ std::map MapsManager::updateMapCaches( std::map filteredPoses; // update cache - if(updateCloud || updateProj || updateGrid || updateScan || updateOctomap) + if(updateGrid || updateOctomap) { // filter nodes if(mapFilterRadius_ > 0.0) @@ -349,29 +382,14 @@ std::map MapsManager::updateMapCaches( bool longUpdate = false; if(filteredPoses.size() > 20) { - if(updateCloud && clouds_.size() < 5) + if(updateGrid && gridMaps_.size() < 5) { - ROS_WARN("Many clouds should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-clouds_.size())); - longUpdate = true; - } - else if(updateProj && projMaps_.size() < 5) - { - ROS_WARN("Many occupancy grid map from projections should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-projMaps_.size())); - longUpdate = true; - } - else if(updateGrid && gridMaps_.size() < 5) - { - ROS_WARN("Many occupancy grid map from laser scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size())); - longUpdate = true; - } - else if(updateScan && scans_.size() < 5) - { - ROS_WARN("Many scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-scans_.size())); + ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size())); longUpdate = true; } #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP - else if(updateOctomap && octomap_->addedNodes().size() < 5) + if(updateOctomap && octomap_->addedNodes().size() < 5) { ROS_WARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size())); longUpdate = true; @@ -385,27 +403,7 @@ std::map MapsManager::updateMapCaches( if(!iter->second.isNull()) { rtabmap::SensorData data; - bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first)); - bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first)); - bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first)); - bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first)); - -#ifdef WITH_OCTOMAP_ROS -#ifdef RTABMAP_OCTOMAP - if(!rgbDepthRequired) - { - rgbDepthRequired = updateOctomap && - (iter->first < 0 || - octomap_->addedNodes().empty() || - iter->first > octomap_->addedNodes().rbegin()->first); - } -#endif -#endif - - if(rgbDepthRequired || - depthRequired || - scanRequired || - gridRequired) + if((updateGrid || updateOctomap) && (iter->first < 0 || !uContains(gridMaps_, iter->first))) { UDEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); @@ -415,277 +413,84 @@ std::map MapsManager::updateMapCaches( } else if(memory) { - data = memory->getSignatureDataConst(iter->first); + data = memory->getSignatureDataConst(iter->first, false, false, false, true); } - } - if(data.id() != 0) - { - if(!(data.imageCompressed().empty() && data.imageRaw().empty()) && - !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) && - (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())) + if(data.id() != 0) { - // Which data should we decompress? - cv::Mat image, depth, scan; + cv::Mat ground, obstacles; data.uncompressData( - (rgbDepthRequired||data.stereoCameraModel().isValidForProjection()) ? &image:0, - (rgbDepthRequired||depthRequired) ? &depth:0, - scanRequired||gridRequired?&scan:0); + 0, + 0, + 0, + 0, + &ground, + &obstacles); - pcl::PointCloud::Ptr cloudRGB; - pcl::PointCloud::Ptr cloudXYZ; - if(rgbDepthRequired) + UDEBUG("Adding grid map %d to cache...", iter->first); + + cv::Point3f viewPoint; + if(iter->first > 0 || data.gridCellSize()) { - UDEBUG("rgbDepthRequired"); - if(!image.empty() && !depth.empty()) + viewPoint = data.gridViewPoint(); + gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles))); + gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint)); + } + else + { + // generate tmp occupancy grid for negative ids + // we need the signature + std::map::const_iterator findIter = signatures.find(iter->first); + if(findIter != signatures.end()) { - pcl::IndicesPtr validIndices(new std::vector); - cloudRGB = util3d::cloudRGBFromSensorData( - data, - cloudDecimation_, - cloudMaxDepth_, - cloudMinDepth_, - validIndices.get()); - if(cloudVoxelSize_) - { - cloudRGB = util3d::voxelize(cloudRGB, validIndices, cloudVoxelSize_); - } - if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) - { - pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::copyPointCloud(*cloudRGB, *indices, *tmp); - cloudRGB = tmp; - } + // normally data should be already uncompressed for negative ids + occupancyGrid_->createLocalMap(findIter->second, ground, obstacles, viewPoint); + gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles))); + gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint)); } else { - ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first); + ROS_WARN("%d signature not found in cache?!?!?", iter->first); } } - else if(depthRequired) + if(ground.cols || obstacles.cols) { - UDEBUG("depthRequired"); - if( !depth.empty()) - { - pcl::IndicesPtr validIndices(new std::vector); - cloudXYZ = util3d::cloudFromSensorData( - data, - cloudDecimation_, - cloudMaxDepth_, - cloudMinDepth_, - validIndices.get()); // use gridCellSize since this cloud is only for the projection map - UASSERT(gridCellSize_ > 0); - cloudXYZ = util3d::voxelize(cloudXYZ, validIndices, gridCellSize_); - if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) - { - pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::copyPointCloud(*cloudXYZ, *indices, *tmp); - cloudXYZ = tmp; - } - } - else - { - ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first); - } - } - - if(cloudRGB.get()) - { - uInsert(clouds_, std::make_pair(iter->first, cloudRGB)); - - // Make sure that image size is set in camera models. - // The camera models are used when cloud_frustum_culling=true. - std::vector models; - if(data.stereoCameraModel().isValidForProjection()) - { - //insert only the left camera model - rtabmap::CameraModel model = data.stereoCameraModel().left(); - model.setImageSize(cv::Size(data.imageRaw().cols, data.imageRaw().rows)); - models.push_back(model); - } - else if(data.cameraModels().size()) - { - UASSERT_MSG(data.imageRaw().cols % data.cameraModels().size() == 0, - uFormat("data.imageRaw().cols=%d data.cameraModels().size()=%d", - data.imageRaw().cols, (int)data.cameraModels().size()).c_str()); - - models.resize(data.cameraModels().size()); - for(unsigned int i=0; ifirst, models)); - } - - if(depthRequired || updateOctomap) - { - UDEBUG("Creating proj map / octomap for %d...", iter->first); - cv::Mat ground, obstacles; - if(cloudRGB.get()) - { - pcl::PointCloud::Ptr cloudClipped = cloudRGB; - if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) - { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); - } - if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_) - { - cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); - } - if(cloudClipped->size()) - { - // add pose rotation without yaw - float roll, pitch, yaw; - iter->second.getEulerAngles(roll, pitch, yaw); - cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0)); - - pcl::IndicesPtr groundIndices, obstaclesIndices; - util3d::segmentObstaclesFromGround( - cloudClipped, - groundIndices, - obstaclesIndices, - 20, - projMaxGroundAngle_*M_PI/180.0, - gridCellSize_*2.0f, - projMinClusterSize_, - projDetectFlatObstacles_, - projMaxGroundHeight_); - - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - - if(groundIndices->size()) - { - pcl::copyPointCloud(*cloudClipped, *groundIndices, *groundCloud); - } - - if(obstaclesIndices->size()) - { - pcl::copyPointCloud(*cloudClipped, *obstaclesIndices, *obstaclesCloud); - } - - if(updateProj) - { - util3d::occupancy2DFromGroundObstacles( - groundCloud, - obstaclesCloud, - ground, - obstacles, - gridCellSize_); - uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); - } - -#ifdef WITH_OCTOMAP_ROS -#ifdef RTABMAP_OCTOMAP - if(updateOctomap) - { - Transform tinv = Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0).inverse(); - groundCloud = util3d::transformPointCloud(groundCloud, tinv); - obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv); - if(octomapGroundIsObstacle_) - { - *obstaclesCloud += *groundCloud; - groundCloud->clear(); - } - octomap_->addToCache(iter->first, groundCloud, obstaclesCloud); - } -#endif -#endif - } - } - else if(updateProj && cloudXYZ.get()) - { - pcl::PointCloud::Ptr cloudClipped = cloudXYZ; - if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) - { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); - } - if(cloudClipped->size()) - { - // add pose rotation without yaw - float roll, pitch, yaw; - iter->second.getEulerAngles(roll, pitch, yaw); - cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0)); - - pcl::IndicesPtr groundIndices, obstaclesIndices; - util3d::segmentObstaclesFromGround( - cloudClipped, - groundIndices, - obstaclesIndices, - 20, - projMaxGroundAngle_*M_PI/180.0, - gridCellSize_*2.0f, - projMinClusterSize_, - projDetectFlatObstacles_, - projMaxGroundHeight_); - - util3d::occupancy2DFromGroundObstacles( - cloudClipped, - groundIndices, - obstaclesIndices, - ground, - obstacles, - gridCellSize_); - uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); - } - } - } - - if(scanRequired || gridRequired) - { - if(scan.cols && (scanRequired || scanVoxelSize_ > 0.0 || scanDecimation_ > 1)) - { - if(scanDecimation_ > 1) - { - scan = util3d::downsample(scan, scanDecimation_); - } - - if(scanRequired || scanVoxelSize_ > 0.0) - { - pcl::PointCloud::Ptr scanCloud = util3d::laserScanToPointCloud(scan); - if(scanVoxelSize_ > 0.0) - { - scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_); - if(gridRequired && scan.type() == CV_32FC2) - { - scan = util3d::laserScan2dFromPointCloud(*scanCloud); - } - } - - if(scanRequired) - { - uInsert(scans_, std::make_pair(iter->first, scanCloud)); - } - } - } - - if(gridRequired && scan.type() == CV_32FC2) - { - cv::Mat ground, obstacles; - util3d::occupancy2DFromLaserScan( - scan, - ground, - obstacles, - gridCellSize_, - data.id() < 0 || gridUnknownSpaceFilled_, - data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange()); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); - } + occupancyGrid_->addToCache(iter->first, ground, obstacles); } } else { - ROS_ERROR("Some data missing for node %d to update the maps (image=%d, depth=%d, camera=%d)", - iter->first, - !(data.imageCompressed().empty() && data.imageRaw().empty())?1:0, - !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty())?1:0, - (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())?1:0); + ROS_ERROR("Data missing for node %d to update the maps", iter->first); } } + +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + if(updateOctomap && + (iter->first < 0 || + octomap_->addedNodes().empty() || + iter->first > octomap_->addedNodes().rbegin()->first)) + { + std::map >::iterator mter = gridMaps_.find(iter->first); + std::map::iterator pter = gridMapsViewpoints_.find(iter->first); + if(mter != gridMaps_.end() && pter!=gridMapsViewpoints_.end()) + { + if((mter->second.first.empty() || mter->second.first.channels() > 2) && + (mter->second.second.empty() || mter->second.second.channels() > 2)) + { + octomap_->addToCache(iter->first, mter->second.first, mter->second.second, pter->second); + } + else if(!mter->second.first.empty() && !mter->second.second.empty()) + { + ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + iter->first); + } + } + } +#endif +#endif } else { @@ -703,50 +508,12 @@ std::map MapsManager::updateMapCaches( } #endif #endif - - // cleanup not used nodes - UDEBUG("Cleanup not used nodes"); - for(std::map::Ptr >::iterator iter=clouds_.begin(); - iter!=clouds_.end();) - { - if(!uContains(poses, iter->first)) - { - clouds_.erase(iter++); - } - else - { - ++iter; - } - } - for(std::map::Ptr >::iterator iter=scans_.begin(); - iter!=scans_.end();) - { - if(!uContains(poses, iter->first)) - { - scans_.erase(iter++); - } - else - { - ++iter; - } - } - for(std::map >::iterator iter=projMaps_.begin(); - iter!=projMaps_.end();) - { - if(!uContains(poses, iter->first)) - { - projMaps_.erase(iter++); - } - else - { - ++iter; - } - } for(std::map >::iterator iter=gridMaps_.begin(); iter!=gridMaps_.end();) { if(!uContains(poses, iter->first)) { + UASSERT(gridMapsViewpoints_.erase(iter->first) != 0); gridMaps_.erase(iter++); } else @@ -754,18 +521,6 @@ std::map MapsManager::updateMapCaches( ++iter; } } - for(std::map >::iterator iter=cameraModels_.begin(); - iter!=cameraModels_.end();) - { - if(!uContains(poses, iter->first)) - { - cameraModels_.erase(iter++); - } - else - { - ++iter; - } - } if(longUpdate) { @@ -784,108 +539,168 @@ void MapsManager::publishMaps( UDEBUG("Publishing maps..."); // publish maps - if(cloudMapPub_.getNumSubscribers()) + if(cloudMapPub_.getNumSubscribers() || + scanMapPub_.getNumSubscribers() || + cloudObstaclesPub_.getNumSubscribers() || + cloudGroundPub_.getNumSubscribers()) { // generate the assembled cloud! UTimer time; - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - int count = 0; - std::list > negativePoses; + + if(scanMapPub_.getNumSubscribers()) + { + if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() && + uStr2Bool(parameters_.at(Parameters::kGridFromDepth()))) + { + ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic " + "instead with . " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + Parameters::kGridFromDepth().c_str()); + } + else + { + ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic instead."); + } + } + + // detect if the graph has changed, if so, recreate the clouds + bool graphGroundChanged = false; + bool graphObstacleChanged = false; + bool updateGround = cloudMapPub_.getNumSubscribers() || + scanMapPub_.getNumSubscribers() || + cloudGroundPub_.getNumSubscribers(); + bool updateObstacles = cloudMapPub_.getNumSubscribers() || + scanMapPub_.getNumSubscribers() || + cloudObstaclesPub_.getNumSubscribers(); + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + std::map::const_iterator jter; + if(updateGround) + { + jter = assembledGroundPoses_.find(iter->first); + if(jter != assembledGroundPoses_.end()) + { + UASSERT(!iter->second.isNull() && !jter->second.isNull()); + if(iter->second.getDistanceSquared(jter->second) > 0.0001) + { + graphGroundChanged = true; + } + } + } + if(updateObstacles) + { + jter = assembledObstaclePoses_.find(iter->first); + if(jter != assembledObstaclePoses_.end()) + { + UASSERT(!iter->second.isNull() && !jter->second.isNull()); + if(iter->second.getDistanceSquared(jter->second) > 0.0001) + { + graphObstacleChanged = true; + } + } + } + } + int countObstacles = 0; + int countGrounds = 0; + if(graphGroundChanged) + { + assembledGround_->clear(); + assembledGroundPoses_.clear(); + } + if(graphObstacleChanged) + { + assembledObstacles_->clear(); + assembledObstaclePoses_.clear(); + } for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { if(iter->first > 0) { - std::map::Ptr >::iterator jter = clouds_.find(iter->first); - if(jter != clouds_.end()) + std::map >::iterator jter = gridMaps_.find(iter->first); + if(updateGround && + (graphGroundChanged || assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end())) { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - ++count; + assembledGroundPoses_.insert(*iter); + if(jter!=gridMaps_.end() && jter->second.first.cols) + { + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first, iter->second); + *assembledGround_+=*transformed; + ++countGrounds; + } + } + if(updateObstacles && + (graphObstacleChanged || assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end())) + { + assembledObstaclePoses_.insert(*iter); + if(jter!=gridMaps_.end() && jter->second.second.cols) + { + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.second, iter->second); + *assembledObstacles_+=*transformed; + ++countObstacles; + } } } - else + } + + if(cloudOutputVoxelized_) + { + UASSERT(gridCellSize_ > 0.0); + if(countGrounds && assembledGround_->size()) { - negativePoses.push_back(*iter); + assembledGround_ = util3d::voxelize(assembledGround_, gridCellSize_); } - } - - for(std::list >::reverse_iterator iter=negativePoses.rbegin(); iter!=negativePoses.rend(); ++iter) - { - std::map::Ptr >::iterator jter = clouds_.find(iter->first); - - if(jter != clouds_.end() && jter->second->size()) + if(countObstacles && assembledObstacles_->size()) { - std::map >::iterator kter = cameraModels_.find(iter->first); - if(cloudFrustumCulling_ && kter != cameraModels_.end() && assembledCloud->size()) - { - for(unsigned int i=0; isecond.size(); ++i) - { - if(kter->second[i].isValidForProjection()) - { - assembledCloud = util3d::frustumFiltering( - assembledCloud, - iter->second, // FIXME: should include camera local transform - kter->second[i].horizontalFOV(), - kter->second[i].verticalFOV(), - 0.0f, - cloudMaxDepth_>0.0?cloudMaxDepth_:999999., - true); - //ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size()); - - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - ++count; - } - } - } - else - { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - if(assembledCloud->size()) - { - *assembledCloud+=*transformed; - } - else - { - assembledCloud = transformed; - } - ++count; - } + assembledObstacles_ = util3d::voxelize(assembledObstacles_, gridCellSize_); } } - if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0)) - { - assembledCloud = util3d::passThrough(assembledCloud, "z", - cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0, - cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0); - } + ROS_INFO("Assembled %d obstacle and %d ground clouds (%d points, %fs)", + countObstacles, countGrounds, (int)(assembledGround_->size() + assembledObstacles_->size()), time.ticks()); - if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_) - { - assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_); - } - - ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks()); - - if(assembledCloud->size()) + if(cloudGroundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*assembledCloud, *cloudMsg); + pcl::toROSMsg(*assembledGround_, *cloudMsg); cloudMsg->header.stamp = stamp; cloudMsg->header.frame_id = mapFrameId; - cloudMapPub_.publish(cloudMsg); + cloudGroundPub_.publish(cloudMsg); } - else if(poses.size() - negativePoses.size()) + if(cloudObstaclesPub_.getNumSubscribers()) { - ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size()); + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(*assembledObstacles_, *cloudMsg); + cloudMsg->header.stamp = stamp; + cloudMsg->header.frame_id = mapFrameId; + cloudObstaclesPub_.publish(cloudMsg); + } + if(cloudMapPub_.getNumSubscribers() || scanMapPub_.getNumSubscribers()) + { + pcl::PointCloud cloud = *assembledObstacles_ + *assembledGround_; + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(cloud, *cloudMsg); + cloudMsg->header.stamp = stamp; + cloudMsg->header.frame_id = mapFrameId; + + if(cloudMapPub_.getNumSubscribers()) + { + cloudMapPub_.publish(cloudMsg); + } + if(scanMapPub_.getNumSubscribers()) + { + scanMapPub_.publish(cloudMsg); + } } } else if(mapCacheCleanup_) { - clouds_.clear(); - cameraModels_.clear(); + assembledGround_->clear(); + assembledObstacles_->clear(); + assembledGroundPoses_.clear(); + assembledObstaclePoses_.clear(); } + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP if(octoMapPubBin_.getNumSubscribers() || @@ -913,14 +728,14 @@ void MapsManager::publishMaps( if(octoMapCloud_.getNumSubscribers() || octoMapEmptySpace_.getNumSubscribers()) { sensor_msgs::PointCloud2 msg; - pcl::IndicesPtr obstacles(new std::vector); - pcl::IndicesPtr ground(new std::vector); - pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacles.get(), ground.get()); + pcl::IndicesPtr obstacleIndices(new std::vector); + pcl::IndicesPtr emptyIndices(new std::vector); + pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get()); if(octoMapCloud_.getNumSubscribers()) { pcl::PointCloud cloudObstacles; - pcl::copyPointCloud(*cloud, *obstacles, cloudObstacles); + pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles); pcl::toROSMsg(cloudObstacles, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; @@ -928,9 +743,9 @@ void MapsManager::publishMaps( } if(octoMapEmptySpace_.getNumSubscribers()) { - pcl::PointCloud cloudGround; - pcl::copyPointCloud(*cloud, *ground, cloudGround); - pcl::toROSMsg(cloudGround, msg); + pcl::PointCloud cloudEmptySpace; + pcl::copyPointCloud(*cloud, *emptyIndices, cloudEmptySpace); + pcl::toROSMsg(cloudEmptySpace, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; octoMapEmptySpace_.publish(msg); @@ -970,109 +785,40 @@ void MapsManager::publishMaps( } else if(poses.size()) { - ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size()); + ROS_WARN("Octomap projection map is empty! (poses=%d octomap nodes=%d). " + "Make sure you activated \"%s\" and \"%s\" to true. " + "See \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" for more info.", + (int)poses.size(), (int)octomap_->octree()->size(), + Parameters::kGrid3D().c_str(), Parameters::kGridFromDepth().c_str()); } } } - else + else if(mapCacheCleanup_) { octomap_->clear(); } #endif #endif - - if(scanMapPub_.getNumSubscribers()) + if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers()) { - // generate the assembled scan cloud! - UTimer time; - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - int count = 0; - std::list > negativePoses; - for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + if(projMapPub_.getNumSubscribers()) { - if(iter->first > 0) + if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() && + !uStr2Bool(parameters_.at(Parameters::kGridFromDepth()))) { - std::map::Ptr >::iterator jter = scans_.find(iter->first); - if(jter != scans_.end() && jter->second->size()) - { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - ++count; - } + ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic " + "instead with . " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + Parameters::kGridFromDepth().c_str()); } - // negative poses are not used - } - - if(assembledCloud->size()) - { - if(assembledCloud->size() && scanVoxelSize_ > 0 && scanOutputVoxelized_) + else { - assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_); + ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic instead."); } - - ROS_INFO("Assembled %d scans (%fs)", count, time.ticks()); - - sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*assembledCloud, *cloudMsg); - cloudMsg->header.stamp = stamp; - cloudMsg->header.frame_id = mapFrameId; - scanMapPub_.publish(cloudMsg); } - else if(poses.size()) - { - ROS_WARN("Scan map is empty! (poses=%d, scans=%d)", (int)poses.size(), (int)scans_.size()); - } - } - else if(mapCacheCleanup_) - { - scans_.clear(); - } - if(projMapPub_.getNumSubscribers()) - { - // create the projection map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = this->generateProjMap(poses, xMin, yMin, gridCellSize); - - if(!pixels.empty()) - { - //init - nav_msgs::OccupancyGrid map; - map.info.resolution = gridCellSize; - map.info.origin.position.x = 0.0; - map.info.origin.position.y = 0.0; - map.info.origin.position.z = 0.0; - map.info.origin.orientation.x = 0.0; - map.info.origin.orientation.y = 0.0; - map.info.origin.orientation.z = 0.0; - map.info.origin.orientation.w = 1.0; - - map.info.width = pixels.cols; - map.info.height = pixels.rows; - map.info.origin.position.x = xMin; - map.info.origin.position.y = yMin; - map.data.resize(map.info.width * map.info.height); - - memcpy(map.data.data(), pixels.data, map.info.width * map.info.height); - - map.header.frame_id = mapFrameId; - map.header.stamp = stamp; - - projMapPub_.publish(map); - } - else if(poses.size()) - { - ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size()); - } - } - else if(mapCacheCleanup_) - { - projMaps_.clear(); - } - - if(gridMapPub_.getNumSubscribers()) - { // create the grid map float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; cv::Mat pixels = this->generateGridMap(poses, xMin, yMin, gridCellSize); @@ -1101,36 +847,28 @@ void MapsManager::publishMaps( map.header.frame_id = mapFrameId; map.header.stamp = stamp; - gridMapPub_.publish(map); + if(gridMapPub_.getNumSubscribers()) + { + gridMapPub_.publish(map); + } + if(projMapPub_.getNumSubscribers()) + { + projMapPub_.publish(map); + } } else if(poses.size()) { ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size()); } } - else if(mapCacheCleanup_) + + if(!this->hasSubscribers() && mapCacheCleanup_) { gridMaps_.clear(); + gridMapsViewpoints_.clear(); } } -cv::Mat MapsManager::generateProjMap( - const std::map & poses, - float & xMin, - float & yMin, - float & gridCellSize) -{ - gridCellSize = gridCellSize_; - return util3d::create2DMapFromOccupancyLocalMaps( - poses, - projMaps_, - gridCellSize_, - xMin, yMin, - gridSize_, - gridEroded_, - footprintRadius_); -} - cv::Mat MapsManager::generateGridMap( const std::map & poses, float & xMin, @@ -1138,14 +876,23 @@ cv::Mat MapsManager::generateGridMap( float & gridCellSize) { gridCellSize = gridCellSize_; - cv::Mat map = util3d::create2DMapFromOccupancyLocalMaps( - poses, - gridMaps_, - gridCellSize_, - xMin, yMin, - gridSize_, - gridEroded_, - footprintRadius_); + cv::Mat map; + if(gridIncremental_) + { + occupancyGrid_->update(poses, gridSize_, footprintRadius_); + map = occupancyGrid_->getMap(xMin, yMin); + } + else + { + map = util3d::create2DMapFromOccupancyLocalMaps( + poses, + gridMaps_, + gridCellSize_, + xMin, yMin, + gridSize_, + gridEroded_, + footprintRadius_); + } return map; } diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 9d8f8431..fba69cc9 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -332,16 +332,42 @@ rtabmap::CameraModel cameraModelFromROS( const sensor_msgs::CameraInfo & camInfo, const rtabmap::Transform & localTransform) { - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(camInfo); + cv::Mat D; + if(camInfo.D.size()) + { + D = cv::Mat(1, camInfo.D.size(), CV_64FC1); + memcpy(D.data, camInfo.D.data(), D.cols*sizeof(double)); + } + + cv:: Mat K; + UASSERT(camInfo.K.empty() || camInfo.K.size() == 9); + if(!camInfo.K.empty()) + { + K = cv::Mat(3, 3, CV_64FC1); + memcpy(K.data, camInfo.K.elems, 9*sizeof(double)); + } + + cv:: Mat R; + UASSERT(camInfo.R.empty() || camInfo.R.size() == 9); + if(!camInfo.R.empty()) + { + R = cv::Mat(3, 3, CV_64FC1); + memcpy(R.data, camInfo.R.elems, 9*sizeof(double)); + } + + cv:: Mat P; + UASSERT(camInfo.P.empty() || camInfo.P.size() == 12); + if(!camInfo.P.empty()) + { + P = cv::Mat(3, 4, CV_64FC1); + memcpy(P.data, camInfo.P.elems, 12*sizeof(double)); + } + return rtabmap::CameraModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform, - 0.0, - cv::Size(model.fullResolution().width, model.fullResolution().height)); + "ros", + cv::Size(camInfo.width, camInfo.height), + K, D, R, P, + localTransform); } void cameraModelToROS( const rtabmap::CameraModel & model, @@ -401,16 +427,11 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::CameraInfo & rightCamInfo, const rtabmap::Transform & localTransform) { - image_geometry::StereoCameraModel model; - model.fromCameraInfo(leftCamInfo, rightCamInfo); return rtabmap::StereoCameraModel( - model.left().fx(), - model.left().fy(), - model.left().cx(), - model.left().cy(), - model.baseline(), - localTransform, - cv::Size(model.left().fullResolution().width, model.left().fullResolution().height)); + "ros", + cameraModelFromROS(leftCamInfo, localTransform), + cameraModelFromROS(rightCamInfo, localTransform), + rtabmap::Transform()); } void mapDataFromROS( @@ -587,8 +608,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) stereoModel.isValidForProjection()? rtabmap::SensorData( compressedMatFromBytes(msg.laserScan), - msg.laserScanMaxPts, - msg.laserScanMaxRange, + rtabmap::LaserScanInfo( + msg.laserScanMaxPts, + msg.laserScanMaxRange, + transformFromGeometryMsg(msg.laserScanLocalTransform)), compressedMatFromBytes(msg.image), compressedMatFromBytes(msg.depth), stereoModel, @@ -597,8 +620,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) compressedMatFromBytes(msg.userData)): rtabmap::SensorData( compressedMatFromBytes(msg.laserScan), - msg.laserScanMaxPts, - msg.laserScanMaxRange, + rtabmap::LaserScanInfo( + msg.laserScanMaxPts, + msg.laserScanMaxRange, + transformFromGeometryMsg(msg.laserScanLocalTransform)), compressedMatFromBytes(msg.image), compressedMatFromBytes(msg.depth), models, @@ -608,6 +633,11 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) s.setWords(words); s.setWords3(words3D); s.setWordsDescriptors(wordsDescriptors); + s.sensorData().setOccupancyGrid( + compressedMatFromBytes(msg.grid_ground), + compressedMatFromBytes(msg.grid_obstacles), + msg.grid_cell_size, + point3fFromROS(msg.grid_view_point)); return s; } void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg) @@ -624,8 +654,13 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth); compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan); compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData); - msg.laserScanMaxPts = signature.sensorData().laserScanMaxPts(); - msg.laserScanMaxRange = signature.sensorData().laserScanMaxRange(); + compressedMatToBytes(signature.sensorData().gridGroundCellsCompressed(), msg.grid_ground); + compressedMatToBytes(signature.sensorData().gridObstacleCellsCompressed(), msg.grid_obstacles); + point3fToROS(signature.sensorData().gridViewPoint(), msg.grid_view_point); + msg.grid_cell_size = signature.sensorData().gridCellSize(); + msg.laserScanMaxPts = signature.sensorData().laserScanInfo().maxPoints(); + msg.laserScanMaxRange = signature.sensorData().laserScanInfo().maxRange(); + transformToGeometryMsg(signature.sensorData().laserScanInfo().localTransform(), msg.laserScanLocalTransform); msg.baseline = 0; if(signature.sensorData().cameraModels().size()) { diff --git a/src/nodelets/icp_odometry.cpp b/src/nodelets/icp_odometry.cpp index 72aeee4b..6c96fcb2 100644 --- a/src/nodelets/icp_odometry.cpp +++ b/src/nodelets/icp_odometry.cpp @@ -93,9 +93,10 @@ private: void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg) { // make sure the frame of the laser is updated too - if(getTransform(this->frameId(), + Transform localScanTransform = getTransform(this->frameId(), scanMsg->header.frame_id, - scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull()) + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); + if(localScanTransform.isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); return; @@ -104,7 +105,7 @@ private: //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(this->frameId(), *scanMsg, scanOut, this->tfListener()); + projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -112,8 +113,7 @@ private: rtabmap::SensorData data( scan, - (int)scanMsg->ranges.size(), - scanMsg->range_max, + LaserScanInfo((int)scanMsg->ranges.size(), scanMsg->range_max, localScanTransform), cv::Mat(), cv::Mat(), CameraModel(), @@ -147,10 +147,6 @@ private: { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!localScanTransform.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, localScanTransform); - } scan = util3d::laserScanFromPointCloud(*pclScan); } else @@ -158,11 +154,6 @@ private: pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!localScanTransform.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, localScanTransform); - } - if(scanCloudNormalK_ > 0) { //compute normals @@ -179,8 +170,7 @@ private: rtabmap::SensorData data( scan, - scanCloudMaxPoints_, - 0, + LaserScanInfo(scanCloudMaxPoints_, 0, localScanTransform), cv::Mat(), cv::Mat(), CameraModel(), diff --git a/src/nodelets/rgbdicp_odometry.cpp b/src/nodelets/rgbdicp_odometry.cpp index 3431b8d6..80f0de44 100644 --- a/src/nodelets/rgbdicp_odometry.cpp +++ b/src/nodelets/rgbdicp_odometry.cpp @@ -266,12 +266,14 @@ private: cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); cv::Mat scan; + Transform localScanTransform = Transform::getIdentity(); if(scanMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(this->frameId(), + localScanTransform = getTransform(this->frameId(), scanMsg->header.frame_id, - scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull()) + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); + if(localScanTransform.isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); return; @@ -280,7 +282,7 @@ private: //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(this->frameId(), *scanMsg, scanOut, this->tfListener()); + projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -297,7 +299,7 @@ private: break; } } - Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); + localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); if(localScanTransform.isNull()) { ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec()); @@ -308,10 +310,6 @@ private: { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!localScanTransform.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, localScanTransform); - } scan = util3d::laserScanFromPointCloud(*pclScan); } else @@ -319,11 +317,6 @@ private: pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!localScanTransform.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, localScanTransform); - } - if(scanCloudNormalK_ > 0) { //compute normals @@ -341,8 +334,10 @@ private: rtabmap::SensorData data( scan, - scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0, - scanMsg.get() != 0?scanMsg->range_max:0, + LaserScanInfo( + scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0, + scanMsg.get() != 0?scanMsg->range_max:0, + localScanTransform), ptrImage->image, ptrDepth->image, rtabmapModel,