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

fixed #450

Browse Source
This commit is contained in:
matlabbe
2020-08-06 09:15:28 -04:00
parent 2806baee55
commit e96f72d4ab
2 changed files with 9 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+3 -1
View File
@@ -754,7 +754,9 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
for(std::map<int, cv::Point3f>::const_iterator iter=info.localMap.begin(); iter!=info.localMap.end(); ++iter) for(std::map<int, cv::Point3f>::const_iterator iter=info.localMap.begin(); iter!=info.localMap.end(); ++iter)
{ {
bool inlier = info.words.find(iter->first) != info.words.end(); bool inlier = info.words.find(iter->first) != info.words.end();
pcl::PointXYZRGB pt(inlier?0:255, 255, 0); pcl::PointXYZRGB pt;
pt.r = inlier?0:255;
pt.g = 255;
pt.x = iter->second.x; pt.x = iter->second.x;
pt.y = iter->second.y; pt.y = iter->second.y;
pt.z = iter->second.z; pt.z = iter->second.z;
src/WifiSignalSubNode.cpp
+6 -2
View File
@@ -241,7 +241,10 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
int quality = dBm2Quality(iter->second)*120/100; int quality = dBm2Quality(iter->second)*120/100;
float r,g,b; float r,g,b;
HSVtoRGB(&r,&g,&b,quality,1,1); HSVtoRGB(&r,&g,&b,quality,1,1);
pcl::PointXYZRGB anchor(r*255, g*255, b*255); pcl::PointXYZRGB anchor;
anchor.r = r*255;
anchor.g = g*255;
anchor.b = b*255;
cloud->push_back(anchor); cloud->push_back(anchor);
} }
else else
@@ -272,7 +275,8 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
} }
cloud->push_back(pt); cloud->push_back(pt);
} }
pcl::PointXYZRGB anchor(255, 0, 0); pcl::PointXYZRGB anchor;
anchor.r = 255;
cloud->push_back(anchor); cloud->push_back(anchor);
} }