i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 09:17:47 +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/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)