i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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())
{
std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(iter->second);
UASSERT(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)
if(poseIter == poses.end())
{
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
{
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())