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

updating files to build ros package with rtab-map lib v0.20.3

Browse Source
This commit is contained in:
mirellameelo
2020-10-04 21:02:53 -03:00
parent d771ff566f
commit dea88211de
2 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+6 -6
View File
@@ -1883,10 +1883,10 @@ void CoreWrapper::process(
if(maxMappingNodes_ > 0 && filteredPoses.size()>1) if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
{ {
std::map<int, Transform> nearestPoses; std::map<int, Transform> nearestPoses;
std::vector<int> nodes = graph::findNearestNodes(filteredPoses, mapToOdom_*odom, maxMappingNodes_); std::map<int, float> nodes = graph::findNearestNodes(filteredPoses, mapToOdom_*odom, maxMappingNodes_);
for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{ {
std::map<int, Transform>::iterator pter = filteredPoses.find(*iter); std::map<int, Transform>::iterator pter = filteredPoses.find(iter->first);
if(pter != filteredPoses.end()) if(pter != filteredPoses.end())
{ {
nearestPoses.insert(*pter); nearestPoses.insert(*pter);
@@ -2770,10 +2770,10 @@ void CoreWrapper::publishMapCallback(
if(maxMappingNodes_ > 0 && filteredPoses.size()>1) if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
{ {
std::map<int, Transform> nearestPoses; std::map<int, Transform> nearestPoses;
std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_); std::map<int, float> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{ {
std::map<int, Transform>::iterator pter = filteredPoses.find(*iter); std::map<int, Transform>::iterator pter = filteredPoses.find(iter->first);
if(pter != filteredPoses.end()) if(pter != filteredPoses.end())
{ {
nearestPoses.insert(*pter); nearestPoses.insert(*pter);
src/OdometryROS.cpp
+3 -1
View File
@@ -654,7 +654,9 @@ void OdometryROS::processData(const SensorData & data, const rclcpp::Time & stam
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;