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

Merge branch 'master' of https://github.com/introlab/rtabmap_ros

Browse Source
This commit is contained in:
matlabbe
2016-03-15 17:06:45 -04:00
parent f80b48bee9 99ab405140
commit c5895f61ea
1 changed files with 8 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/MapCloudDisplay.cpp
+8 -7
View File
@@ -256,11 +256,18 @@ void MapCloudDisplay::processMessage( const rtabmap_ros::MapDataConstPtr& msg )
void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
{ {
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
{
poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
}
// Add new clouds... // Add new clouds...
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i) for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{ {
int id = map.nodes[i].id; int id = map.nodes[i].id;
if(cloud_infos_.find(id) == cloud_infos_.end()) if(poses.find(id) != poses.end() &&
cloud_infos_.find(id) == cloud_infos_.end())
{ {
// Cloud not added to RVIZ, add it! // Cloud not added to RVIZ, add it!
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]); rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
@@ -311,12 +318,6 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
} }
// Update graph // Update graph
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
{
poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
}
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f) if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
{ {
poses = rtabmap::graph::radiusPosesFiltering(poses, poses = rtabmap::graph::radiusPosesFiltering(poses,