i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

22
corelib/src/Graph.cpp
View File

@@ -1971,18 +1971,24 @@ std::list<std::pair<int, Transform> > computePath(
if(nodeIter == nodes.end()) if(nodeIter == nodes.end())
{ {
std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(iter->second); std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(iter->second);
UASSERT(poseIter != poses.end()); if(poseIter == poses.end())
Node n(iter->second, currentNode->id(), poseIter->second);
n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second));
n.setDistToEnd(n.distFrom(endPose));
nodes.insert(std::make_pair(iter->second, n));
if(updateNewCosts)
{ {
pqmap.insert(std::make_pair(n.totalCost(), n.id())); UERROR("Next pose %d (from %d) should be found in poses! Ignoring it!", iter->second, iter->first);
} }
else else
{ {
pq.push(Pair(n.id(), n.totalCost())); Node n(iter->second, currentNode->id(), poseIter->second);
n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second));
n.setDistToEnd(n.distFrom(endPose));
nodes.insert(std::make_pair(iter->second, n));
if(updateNewCosts)
{
pqmap.insert(std::make_pair(n.totalCost(), n.id()));
}
else
{
pq.push(Pair(n.id(), n.totalCost()));
}
} }
} }
else if(updateNewCosts && nodeIter->second.isOpened()) else if(updateNewCosts && nodeIter->second.isOpened())

8
corelib/src/Rtabmap.cpp
View File

@@ -3249,8 +3249,12 @@ bool Rtabmap::computePath(const Transform & targetPose)
UASSERT(s); UASSERT(s);
for(std::map<int, Link>::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter) 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())); // only add links for which poses are in "nodes"
links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> 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()); UINFO("Time getting links = %fs", timer.ticks());