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

Updated computePath(pose) to avoid links pointing to nodes not in WM

Browse Source
This commit is contained in:
matlabbe
2015-09-15 18:17:38 -04:00
parent 16d998f3b0
commit d7e6a2c0e8
2 changed files with 20 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+6 -2
View File
@@ -3249,8 +3249,12 @@ bool Rtabmap::computePath(const Transform & targetPose)
UASSERT(s);
for(std::map<int, Link>::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter)
{
links.insert(std::make_pair(jter->second.from(), jter->second.to()));
links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <->
// only add links for which poses are in "nodes"
if(uContains(nodes, jter->second.to()))
{
links.insert(std::make_pair(jter->second.from(), jter->second.to()));
//links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> (commented: already added when iterating in nodes)
}
}
}
UINFO("Time getting links = %fs", timer.ticks());