i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added RVIZ plugin "MapCloud" to show incrementally point clouds of the map in RVIZ

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1455 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-04 23:12:34 +00:00
parent f3235ab176
commit 4384dead67
9 changed files with 109 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+1
View File
@@ -839,6 +839,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
msg->loopClosureId = stats.loopClosureId();
msg->localLoopClosureId = stats.localLoopClosureId();
msg->data.header = msg->header;
msg->data.poseIDs.resize(stats.poses().size());
msg->data.poses.resize(stats.poses().size());
int i=0;
src/MapAssemblerNode.cpp
+43 -43
View File
@@ -66,62 +66,62 @@ public:
void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
{
for(unsigned int i=0; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
for(unsigned int i=0; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
{
int id = msg->data.localTransformIDs[i];
if(!uContains(rgbClouds_, id))
{
int id = msg->data.localTransformIDs[i];
if(!uContains(rgbClouds_, id))
rtabmap::Transform localTransform = transformFromGeometryMsg(msg->data.localTransforms[i]);
if(!localTransform.isNull())
{
rtabmap::Transform localTransform = transformFromGeometryMsg(msg->data.localTransforms[i]);
if(!localTransform.isNull())
cv::Mat image, depth;
float depthConstant = 0.0f;
for(unsigned int i=0; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
{
cv::Mat image, depth;
float depthConstant = 0.0f;
if(msg->data.imageIDs[i] == id)
{
image = util3d::uncompressImage(msg->data.images[i].bytes);
break;
}
}
for(unsigned int i=0; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
{
if(msg->data.depthIDs[i] == id)
{
depth = util3d::uncompressImage(msg->data.depths[i].bytes);
break;
}
}
for(unsigned int i=0; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
{
if(msg->data.depthConstantIDs[i] == id)
{
depthConstant = msg->data.depthConstants[i];
break;
}
}
for(unsigned int i=0; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
if(!image.empty() && !depth.empty() && depthConstant > 0.0f)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_);
if(cloudMaxDepth_ > 0)
{
if(msg->data.imageIDs[i] == id)
{
image = util3d::uncompressImage(msg->data.images[i].bytes);
break;
}
cloud = util3d::passThrough(cloud, "z", 0, cloudMaxDepth_);
}
for(unsigned int i=0; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
if(cloudVoxelSize_ > 0)
{
if(msg->data.depthIDs[i] == id)
{
depth = util3d::uncompressImage(msg->data.depths[i].bytes);
break;
}
}
for(unsigned int i=0; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
{
if(msg->data.depthConstantIDs[i] == id)
{
depthConstant = msg->data.depthConstants[i];
break;
}
cloud = util3d::voxelize(cloud, cloudVoxelSize_);
}
if(!image.empty() && !depth.empty() && depthConstant > 0.0f)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_);
cloud = util3d::transformPointCloud(cloud, localTransform);
if(cloudMaxDepth_ > 0)
{
cloud = util3d::passThrough(cloud, "z", 0, cloudMaxDepth_);
}
if(cloudVoxelSize_ > 0)
{
cloud = util3d::voxelize(cloud, cloudVoxelSize_);
}
cloud = util3d::transformPointCloud(cloud, localTransform);
rgbClouds_.insert(std::make_pair(id, cloud));
}
rgbClouds_.insert(std::make_pair(id, cloud));
}
}
}
}
for(unsigned int i=0; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
src/nodelets/data_odom_sync.cpp
+1 -1
View File
@@ -100,5 +100,5 @@ private:
};
PLUGINLIB_DECLARE_CLASS(rtabmap, data_odom_sync, rtabmap::DataOdomSyncNodelet, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap::DataOdomSyncNodelet, nodelet::Nodelet);
}
src/nodelets/data_throttle.cpp
+1 -1
View File
@@ -107,5 +107,5 @@ private:
};
PLUGINLIB_DECLARE_CLASS(rtabmap, data_throttle, rtabmap::DataThrottleNodelet, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap::DataThrottleNodelet, nodelet::Nodelet);
}
src/nodelets/point_cloud_xyzrgb.cpp
+1 -1
View File
@@ -138,6 +138,6 @@ private:
message_filters::Synchronizer<MySyncPolicy> * sync_;
};
PLUGINLIB_DECLARE_CLASS(rtabmap, point_cloud_xyzrgb, rtabmap::PointCloudXYZRGB, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap::PointCloudXYZRGB, nodelet::Nodelet);
}