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

ros: MapCloudDisplay::downloadMap() creating/buffering also clouds of nodes not in the current map

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1673 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-09-08 20:53:18 +00:00
parent 408f8f307f
commit 989ea63acb
1 changed files with 4 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/MapCloudDisplay.cpp
+4 -13
View File
@@ -249,7 +249,6 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
float depthFy = 0.0f; float depthFy = 0.0f;
float depthCx = 0.0f; float depthCx = 0.0f;
float depthCy = 0.0f; float depthCy = 0.0f;
rtabmap::Transform pose;
for(unsigned int i=0; i<map.imageIDs.size() && i<map.images.size(); ++i) for(unsigned int i=0; i<map.imageIDs.size() && i<map.images.size(); ++i)
{ {
@@ -299,16 +298,8 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
break; break;
} }
} }
for(unsigned int i=0; i<map.poseIDs.size() && i<map.poses.size(); ++i)
{
if(map.poseIDs[i] == id)
{
pose = transformFromPoseMsg(map.poses[i]);
break;
}
}
if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f && !pose.isNull()) if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt());
@@ -329,7 +320,7 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
CloudInfoPtr info(new CloudInfo); CloudInfoPtr info(new CloudInfo);
info->message_ = cloudMsg; info->message_ = cloudMsg;
info->pose_ = pose; info->pose_ = rtabmap::Transform::getIdentity();
info->id_ = id; info->id_ = id;
if (transformCloud(info, true)) if (transformCloud(info, true))
@@ -526,11 +517,11 @@ void MapCloudDisplay::downloadMap()
} }
else else
{ {
messageBox->setText(tr("Creating all clouds (%1 nodes downloaded)...").arg(getMapSrv.response.data.poses.size())); messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...").arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.depths.size()));
QApplication::processEvents(); QApplication::processEvents();
this->reset(); this->reset();
processMapData(getMapSrv.response.data); processMapData(getMapSrv.response.data);
messageBox->setText(tr("Creating all clouds (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.poses.size())); messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!").arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.depths.size()));
QTimer::singleShot(1000, messageBox, SLOT(close())); QTimer::singleShot(1000, messageBox, SLOT(close()));
} }