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

8
corelib/src/Graph.cpp
View File

@@ -1971,7 +1971,12 @@ 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())
{
UERROR("Next pose %d (from %d) should be found in poses! Ignoring it!", iter->second, iter->first);
}
else
{
Node n(iter->second, currentNode->id(), poseIter->second); Node n(iter->second, currentNode->id(), poseIter->second);
n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second)); n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second));
n.setDistToEnd(n.distFrom(endPose)); n.setDistToEnd(n.distFrom(endPose));
@@ -1985,6 +1990,7 @@ std::list<std::pair<int, Transform> > computePath(
pq.push(Pair(n.id(), n.totalCost())); pq.push(Pair(n.id(), n.totalCost()));
} }
} }
}
else if(updateNewCosts && nodeIter->second.isOpened()) else if(updateNewCosts && nodeIter->second.isOpened())
{ {
float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose()); float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose());

6
corelib/src/Rtabmap.cpp
View File

@@ -3248,9 +3248,13 @@ bool Rtabmap::computePath(const Transform & targetPose)
const Signature * s = _memory->getSignature(iter->first); const Signature * s = _memory->getSignature(iter->first);
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)
{
// 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.from(), jter->second.to()));
links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> //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());