diff --git a/rtabmap/launch/config/appearance_gui.ini b/rtabmap/launch/config/appearance_gui.ini index c07da50b..332be1de 100644 --- a/rtabmap/launch/config/appearance_gui.ini +++ b/rtabmap/launch/config/appearance_gui.ini @@ -38,3 +38,4 @@ General\voxelSize2=0.01 General\decimation2=1 General\maxDepth2=4 General\showScans2=true +General\meshing0=false diff --git a/rtabmap/msg/Info.msg b/rtabmap/msg/Info.msg index f709aedc..e113f2b3 100644 --- a/rtabmap/msg/Info.msg +++ b/rtabmap/msg/Info.msg @@ -7,11 +7,8 @@ Header header int32 refId -int32 refMapId int32 loopClosureId -int32 loopClosureMapId int32 localLoopClosureId -int32 localLoopClosureMapId # std::map poses; int32[] nodeIds diff --git a/rtabmap/msg/InfoEx.msg b/rtabmap/msg/InfoEx.msg index 287e3f05..245c71a0 100644 --- a/rtabmap/msg/InfoEx.msg +++ b/rtabmap/msg/InfoEx.msg @@ -6,21 +6,18 @@ Header header int32 refId -int32 refMapId int32 loopClosureId -int32 loopClosureMapId int32 localLoopClosureId -int32 localLoopClosureMapId -# std::map poses; -int32[] nodeIds -geometry_msgs/Pose[] nodePoses +# The map data (rgb, depth, depthConstant, depth2D, localTransform, poses, map ids, mapCorrection) +rtabmap/MapData data -geometry_msgs/Transform mapCorrection geometry_msgs/Transform loopClosureTransform - geometry_msgs/Pose currentPose +#### +# For statistics and visualization below... +#### # std::map posterior; int32[] posteriorKeys float32[] posteriorValues @@ -41,29 +38,6 @@ int32[] weightsValues string[] statsKeys float32[] statsValues -#compressed image -# use rtabmap::util3d::uncompressImage() from -uint8[] refImage -uint8[] loopImage - -#compressed depth -# use rtabmap::util3d::uncompressImage() from -uint8[] refDepth -uint8[] loopDepth - -#compressed depth2d -# use rtabmap::util3d::uncompressData() from -uint8[] refDepth2D -uint8[] loopDepth2D - -#camera info -float32 refDepthConstant -float32 loopDepthConstant - -#local transform -geometry_msgs/Transform refLocalTransform -geometry_msgs/Transform loopLocalTransform - # # For features2d : std::multimap words # diff --git a/rtabmap/msg/MapData.msg b/rtabmap/msg/MapData.msg index a18a0d9d..9f04a8cb 100644 --- a/rtabmap/msg/MapData.msg +++ b/rtabmap/msg/MapData.msg @@ -1,6 +1,10 @@ Header header +# Map ids +int32[] mapIDs +int32[] maps + # compressed images # use rtabmap::util3d::uncompressImage() from # std::map images; @@ -17,7 +21,7 @@ rtabmap/Bytes[] depths # use rtabmap::util3d::uncompressData() from # std::map depths2d; int32[] depth2DIDs -rtabmap/Bytes[] depths2D +rtabmap/Bytes[] depth2Ds # depth constants int32[] depthConstantIDs diff --git a/rtabmap/src/CoreWrapper.cpp b/rtabmap/src/CoreWrapper.cpp index 02dab557..f6d3950c 100644 --- a/rtabmap/src/CoreWrapper.cpp +++ b/rtabmap/src/CoreWrapper.cpp @@ -650,12 +650,12 @@ bool CoreWrapper::publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Em } msg->depth2DIDs.resize(depths2d.size()); - msg->depths2D.resize(depths2d.size()); + msg->depth2Ds.resize(depths2d.size()); i=0; for(std::map >::iterator iter = depths2d.begin(); iter!=depths2d.end(); ++iter) { msg->depth2DIDs[i] = iter->first; - msg->depths2D[i].bytes = iter->second; + msg->depth2Ds[i].bytes = iter->second; ++i; } @@ -709,11 +709,8 @@ void CoreWrapper::publishStats(const Statistics & stats) msg->header.frame_id = mapFrameId_; msg->refId = stats.refImageId(); - msg->refMapId = stats.refImageMapId(); msg->loopClosureId = stats.loopClosureId(); - msg->loopClosureMapId = stats.loopClosureMapId(); msg->localLoopClosureId = stats.localLoopClosureId(); - msg->localLoopClosureMapId = stats.localLoopClosureMapId(); msg->nodeIds.resize(stats.poses().size()); msg->nodePoses.resize(stats.poses().size()); @@ -742,34 +739,28 @@ void CoreWrapper::publishStats(const Statistics & stats) msg->header.frame_id = mapFrameId_; msg->refId = stats.refImageId(); - msg->refMapId = stats.refImageMapId(); msg->loopClosureId = stats.loopClosureId(); - msg->loopClosureMapId = stats.loopClosureMapId(); msg->localLoopClosureId = stats.localLoopClosureId(); - msg->localLoopClosureMapId = stats.localLoopClosureMapId(); - msg->nodeIds.resize(stats.poses().size()); - msg->nodePoses.resize(stats.poses().size()); + msg->data.poseIDs.resize(stats.poses().size()); + msg->data.poses.resize(stats.poses().size()); int i=0; for(std::map::const_iterator iter = stats.poses().begin(); iter!=stats.poses().end(); ++iter) { - msg->nodeIds[i] = iter->first; - transformToPoseMsg(iter->second, msg->nodePoses[i]); + msg->data.poseIDs[i] = iter->first; + transformToPoseMsg(iter->second, msg->data.poses[i]); ++i; } - transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection); + transformToGeometryMsg(stats.mapCorrection(), msg->data.mapCorrection); transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform); transformToPoseMsg(stats.currentPose(), msg->currentPose); // Detailed info if(stats.extended()) { - msg->refImage = stats.refImage(); - msg->loopImage = stats.loopImage(); - //Posterior, likelihood, childCount msg->posteriorKeys = uKeys(stats.posterior()); msg->posteriorValues = uValues(stats.posterior()); @@ -819,17 +810,56 @@ void CoreWrapper::publishStats(const Statistics & stats) msg->statsKeys = uKeys(stats.data()); msg->statsValues = uValues(stats.data()); + msg->data.mapIDs = uKeys(stats.getMapIds()); + msg->data.maps = uValues(stats.getMapIds()); + //RGB-D SLAM data - msg->refDepth = stats.refDepth(); - msg->refDepth2D = stats.refDepth2D(); - msg->loopDepth = stats.loopDepth(); - msg->loopDepth2D = stats.loopDepth2D(); + msg->data.imageIDs.resize(stats.getImages().size()); + msg->data.images.resize(stats.getImages().size()); + index = 0; + for(std::map >::const_iterator i=stats.getImages().begin(); + i!=stats.getImages().end(); + ++i) + { + msg->data.imageIDs[index] = i->first; + msg->data.images[index].bytes = i->second; + } - msg->refDepthConstant = stats.refDepthConstant(); - msg->loopDepthConstant = stats.loopDepthConstant(); + msg->data.depthIDs.resize(stats.getDepths().size()); + msg->data.depths.resize(stats.getDepths().size()); + index = 0; + for(std::map >::const_iterator i=stats.getDepths().begin(); + i!=stats.getDepths().end(); + ++i) + { + msg->data.depthIDs[index] = i->first; + msg->data.depths[index].bytes = i->second; + } - transformToGeometryMsg(stats.refLocalTransform(), msg->refLocalTransform); - transformToGeometryMsg(stats.loopLocalTransform(), msg->loopLocalTransform); + msg->data.depth2DIDs.resize(stats.getDepth2ds().size()); + msg->data.depth2Ds.resize(stats.getDepth2ds().size()); + index = 0; + for(std::map >::const_iterator i=stats.getDepth2ds().begin(); + i!=stats.getDepth2ds().end(); + ++i) + { + msg->data.depth2DIDs[index] = i->first; + msg->data.depth2Ds[index].bytes = i->second; + } + + msg->data.localTransformIDs.resize(stats.getLocalTransforms().size()); + msg->data.localTransforms.resize(stats.getLocalTransforms().size()); + index = 0; + for(std::map::const_iterator i=stats.getLocalTransforms().begin(); + i!=stats.getLocalTransforms().end(); + ++i) + { + msg->data.localTransformIDs[index] = i->first; + transformToGeometryMsg(i->second, msg->data.localTransforms[index]); + } + + msg->data.depthConstantIDs = uKeys(stats.getDepthConstants()); + msg->data.depthConstants = uValues(stats.getDepthConstants()); } infoPubEx_.publish(msg); } diff --git a/rtabmap/src/GridMapAssemblerNode.cpp b/rtabmap/src/GridMapAssemblerNode.cpp index d3b9849f..075c00c8 100644 --- a/rtabmap/src/GridMapAssemblerNode.cpp +++ b/rtabmap/src/GridMapAssemblerNode.cpp @@ -62,16 +62,19 @@ public: void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) { - if(!uContains(scans_, msg->refId) && msg->refDepth2D.size()) + for(unsigned int i=0; idata.depth2DIDs.size() && idata.depth2Ds.size(); ++i) { - cv::Mat depth2d = util3d::uncompressData(msg->refDepth2D); - scans_.insert(std::make_pair(msg->refId, util3d::depth2DToPointCloud(depth2d))); + if(!uContains(scans_, msg->data.depth2DIDs[i])) + { + cv::Mat depth2d = util3d::uncompressData(msg->data.depth2Ds[i].bytes); + scans_.insert(std::make_pair(msg->data.depth2DIDs[i], util3d::depth2DToPointCloud(depth2d))); + } } std::map poses; - for(unsigned int i=0; inodeIds.size() && inodePoses.size(); ++i) + for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) { - poses.insert(std::make_pair(msg->nodeIds[i], transformFromPoseMsg(msg->nodePoses[i]))); + poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i]))); } if(gridMap_.getNumSubscribers()) @@ -114,18 +117,18 @@ public: { std::map > depths2d; - if(msg->depth2DIDs.size() != msg->depths2D.size()) + if(msg->depth2DIDs.size() != msg->depth2Ds.size()) { ROS_WARN("grid_map_assembler: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!", - (int)msg->depths2D.size(), (int)msg->depth2DIDs.size()); + (int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size()); } // fill maps - for(unsigned int i=0; idepth2DIDs.size() && i < msg->depths2D.size(); ++i) + for(unsigned int i=0; idepth2DIDs.size() && i < msg->depth2Ds.size(); ++i) { if(!uContains(scans_, msg->depth2DIDs[i])) { - cv::Mat depth2d = util3d::uncompressData(msg->depths2D[i].bytes); + cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes); scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d))); } } diff --git a/rtabmap/src/GuiWrapper.cpp b/rtabmap/src/GuiWrapper.cpp index 8259aa67..907581c1 100644 --- a/rtabmap/src/GuiWrapper.cpp +++ b/rtabmap/src/GuiWrapper.cpp @@ -111,14 +111,8 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) stat.setExtended(true); // Extended stat.setRefImageId(msg->refId); - stat.setRefImageMapId(msg->refMapId); stat.setLoopClosureId(msg->loopClosureId); - stat.setLoopClosureMapId(msg->loopClosureMapId); stat.setLocalLoopClosureId(msg->localLoopClosureId); - stat.setLocalLoopClosureMapId(msg->localLoopClosureMapId); - - stat.setRefImage(msg->refImage); - stat.setLoopImage(msg->loopImage); //Posterior, likelihood, childCount std::map mapIntFloat; @@ -179,28 +173,59 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) } //RGB-D SLAM data - stat.setRefDepth(msg->refDepth); - stat.setRefDepth2D(msg->refDepth2D); - stat.setLoopDepth(msg->loopDepth); - stat.setLoopDepth2D(msg->loopDepth2D); - - stat.setRefDepthConstant(msg->refDepthConstant); - stat.setLoopDepthConstant(msg->loopDepthConstant); - - stat.setRefLocalTransform(transformFromGeometryMsg(msg->refLocalTransform)); - stat.setLoopLocalTransform(transformFromGeometryMsg(msg->loopLocalTransform)); - - stat.setMapCorrection(transformFromGeometryMsg(msg->mapCorrection)); + stat.setMapCorrection(transformFromGeometryMsg(msg->data.mapCorrection)); stat.setLoopClosureTransform(transformFromGeometryMsg(msg->loopClosureTransform)); stat.setCurrentPose(transformFromPoseMsg(msg->currentPose)); - std::map poses; - for(unsigned int i=0; inodeIds.size() && inodePoses.size(); ++i) + std::map > images; + for(unsigned int i=0; idata.imageIDs.size() && idata.images.size(); ++i) { - poses.insert(std::make_pair(msg->nodeIds[i], transformFromPoseMsg(msg->nodePoses[i]))); + images.insert(std::make_pair(msg->data.imageIDs[i], msg->data.images[i].bytes)); + } + stat.setImages(images); + + std::map > depths; + for(unsigned int i=0; idata.depthIDs.size() && idata.depths.size(); ++i) + { + depths.insert(std::make_pair(msg->data.depthIDs[i], msg->data.depths[i].bytes)); + } + stat.setDepths(depths); + + std::map > depth2ds; + for(unsigned int i=0; idata.depth2DIDs.size() && idata.depth2Ds.size(); ++i) + { + depth2ds.insert(std::make_pair(msg->data.depth2DIDs[i], msg->data.depth2Ds[i].bytes)); + } + stat.setDepth2ds(depth2ds); + + std::map depthConstants; + for(unsigned int i=0; idata.depthConstantIDs.size() && idata.depthConstants.size(); ++i) + { + depthConstants.insert(std::make_pair(msg->data.depthConstantIDs[i], msg->data.depthConstants[i])); + } + stat.setDepthConstants(depthConstants); + + std::map localTransforms; + for(unsigned int i=0; idata.localTransformIDs.size() && idata.localTransforms.size(); ++i) + { + localTransforms.insert(std::make_pair(msg->data.localTransformIDs[i], transformFromGeometryMsg(msg->data.localTransforms[i]))); + } + stat.setLocalTransforms(localTransforms); + + std::map poses; + for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) + { + poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i]))); } stat.setPoses(poses); + std::map mapIds; + for(unsigned int i=0; idata.mapIDs.size() && idata.maps.size(); ++i) + { + mapIds.insert(std::make_pair(msg->data.mapIDs[i], msg->data.maps[i])); + } + stat.setMapIds(mapIds); + this->post(new RtabmapEvent(stat)); } @@ -226,10 +251,10 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) (int)msg->depths.size(), (int)msg->depthIDs.size()); } - if(msg->depth2DIDs.size() != msg->depths2D.size()) + if(msg->depth2DIDs.size() != msg->depth2Ds.size()) { ROS_WARN("rtabmapviz: receiving map... depths2D and IDs are not the same size (%d vs %d)!", - (int)msg->depths2D.size(), (int)msg->depth2DIDs.size()); + (int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size()); } if(msg->depthConstantIDs.size() != msg->depthConstants.size()) @@ -248,9 +273,9 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) depths.insert(std::make_pair(msg->depthIDs[i], msg->depths[i].bytes)); } - for(unsigned int i=0; idepth2DIDs.size() && i < msg->depths2D.size(); ++i) + for(unsigned int i=0; idepth2DIDs.size() && i < msg->depth2Ds.size(); ++i) { - depths2d.insert(std::make_pair(msg->depth2DIDs[i], msg->depths2D[i].bytes)); + depths2d.insert(std::make_pair(msg->depth2DIDs[i], msg->depth2Ds[i].bytes)); } for(unsigned int i=0; idepthConstantIDs.size() && i < msg->depthConstants.size(); ++i) diff --git a/rtabmap/src/MapAssemblerNode.cpp b/rtabmap/src/MapAssemblerNode.cpp index f4b75986..cc00c5fd 100644 --- a/rtabmap/src/MapAssemblerNode.cpp +++ b/rtabmap/src/MapAssemblerNode.cpp @@ -64,50 +64,89 @@ public: void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) { - if(!uContains(rgbClouds_, msg->refId) && msg->refImage.size() && msg->refDepth.size() && msg->refDepthConstant > 0) - { - rtabmap::Transform localTransform = transformFromGeometryMsg(msg->refLocalTransform); - - if(!localTransform.isNull()) + for(unsigned int i=0; idata.localTransformIDs.size() && idata.localTransforms.size(); ++i) { - cv::Mat image = util3d::uncompressImage(msg->refImage); - cv::Mat depth = util3d::uncompressImage(msg->refDepth); - pcl::PointCloud::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, msg->refDepthConstant, cloudDecimation_); - - if(cloudVoxelSize_ > 0) + int id = msg->data.localTransformIDs[i]; + if(!uContains(rgbClouds_, id)) { - cloud = util3d::voxelize(cloud, cloudVoxelSize_); + rtabmap::Transform localTransform = transformFromGeometryMsg(msg->data.localTransforms[i]); + if(!localTransform.isNull()) + { + cv::Mat image, depth; + float depthConstant = 0.0f; + + for(unsigned int i=0; idata.imageIDs.size() && idata.images.size(); ++i) + { + if(msg->data.imageIDs[i] == id) + { + image = util3d::uncompressImage(msg->data.images[i].bytes); + break; + } + } + for(unsigned int i=0; idata.depthIDs.size() && idata.depths.size(); ++i) + { + if(msg->data.depthIDs[i] == id) + { + depth = util3d::uncompressImage(msg->data.depths[i].bytes); + break; + } + } + for(unsigned int i=0; idata.depthConstantIDs.size() && idata.depthConstants.size(); ++i) + { + if(msg->data.depthConstantIDs[i] == id) + { + depthConstant = msg->data.depthConstants[i]; + break; + } + } + + if(!image.empty() && !depth.empty() && depthConstant > 0.0f) + { + pcl::PointCloud::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_); + + if(cloudVoxelSize_ > 0) + { + cloud = util3d::voxelize(cloud, cloudVoxelSize_); + } + + cloud = util3d::transformPointCloud(cloud, localTransform); + + rgbClouds_.insert(std::make_pair(id, cloud)); + } + } } - - cloud = util3d::transformPointCloud(cloud, localTransform); - - rgbClouds_.insert(std::make_pair(msg->refId, cloud)); } - } - if(!uContains(scans_, msg->refId) && msg->refDepth2D.size()) + + for(unsigned int i=0; idata.depth2DIDs.size() && idata.depth2Ds.size(); ++i) { - cv::Mat depth2d = util3d::uncompressData(msg->refDepth2D); - - pcl::PointCloud::Ptr cloud = util3d::depth2DToPointCloud(depth2d); - if(scanVoxelSize_ > 0) + if(!uContains(scans_, msg->data.depth2DIDs[i])) { - cloud = util3d::voxelize(cloud, scanVoxelSize_); - } + cv::Mat depth2d = util3d::uncompressData(msg->data.depth2Ds[i].bytes); + if(!depth2d.empty()) + { + pcl::PointCloud::Ptr cloud = util3d::depth2DToPointCloud(depth2d); + if(scanVoxelSize_ > 0) + { + cloud = util3d::voxelize(cloud, scanVoxelSize_); + } - scans_.insert(std::make_pair(msg->refId, cloud)); + scans_.insert(std::make_pair(msg->data.depth2DIDs[i], cloud)); + } + } } + if(assembledMapClouds_.getNumSubscribers()) { // generate the assembled cloud! pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - for(unsigned int i=0; inodeIds.size() && inodePoses.size(); ++i) + for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) { - Transform pose = transformFromPoseMsg(msg->nodePoses[i]); + Transform pose = transformFromPoseMsg(msg->data.poses[i]); - std::map::Ptr >::iterator iter = rgbClouds_.find(msg->nodeIds[i]); + std::map::Ptr >::iterator iter = rgbClouds_.find(msg->data.poseIDs[i]); if(iter != rgbClouds_.end()) { pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(iter->second, pose); @@ -135,11 +174,11 @@ public: // generate the assembled scan! pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - for(unsigned int i=0; inodeIds.size() && inodePoses.size(); ++i) + for(unsigned int i=0; idata.poseIDs.size() && idata.poses.size(); ++i) { - Transform pose = transformFromPoseMsg(msg->nodePoses[i]); + Transform pose = transformFromPoseMsg(msg->data.poses[i]); - std::map::Ptr >::iterator iter = scans_.find(msg->nodeIds[i]); + std::map::Ptr >::iterator iter = scans_.find(msg->data.poseIDs[i]); if(iter != scans_.end()) { pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(iter->second, pose); @@ -190,10 +229,10 @@ public: (int)msg->depthConstants.size(), (int)msg->depthConstantIDs.size()); } - if(msg->depth2DIDs.size() != msg->depths2D.size()) + if(msg->depth2DIDs.size() != msg->depth2Ds.size()) { ROS_WARN("rtabmapviz: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!", - (int)msg->depths2D.size(), (int)msg->depth2DIDs.size()); + (int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size()); } // fill maps @@ -230,11 +269,11 @@ public: } } - for(unsigned int i=0; idepth2DIDs.size() && i < msg->depths2D.size(); ++i) + for(unsigned int i=0; idepth2DIDs.size() && i < msg->depth2Ds.size(); ++i) { if(!uContains(scans_, msg->depth2DIDs[i])) { - cv::Mat depth2d = util3d::uncompressData(msg->depths2D[i].bytes); + cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes); scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d))); } }