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

Rtabmap::computePath() make sure the closest pose of the current location is a node, not a landmark

Browse Source
This commit is contained in:
matlabbe
2018-12-07 20:43:46 -05:00
parent de32e53868
commit 986db04cb9
1 changed files with 22 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
corelib/src/Rtabmap.cpp
View File

@@ -2461,17 +2461,10 @@ bool Rtabmap::process(
//used in localization mode: filter virtual links //used in localization mode: filter virtual links
std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure); std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
localizationLinks = graph::filterLinks(localizationLinks, Link::kPosePrior); localizationLinks = graph::filterLinks(localizationLinks, Link::kPosePrior);
if(landmarkDetected!=0 && !_memory->isIncremental()) if(landmarkDetected!=0 && !_memory->isIncremental() && _optimizedPoses.find(landmarkDetected)!=_optimizedPoses.end())
{ {
//Add fake link between current node and the node also observing the same landmark
UASSERT(uContains(_optimizedPoses, landmarkDetectedNodeRef));
const Signature * s = _memory->getSignature(landmarkDetectedNodeRef);
UASSERT(s!=0);
UASSERT(uContains(s->getLandmarks(), landmarkDetected));
UASSERT(uContains(signature->getLandmarks(), landmarkDetected)); UASSERT(uContains(signature->getLandmarks(), landmarkDetected));
const Link & landmarkLink = s->getLandmarks().at(landmarkDetected); localizationLinks.insert(std::make_pair(landmarkDetected, signature->getLandmarks().at(landmarkDetected)));
const Link & landmarkLink2 = signature->getLandmarks().at(landmarkDetected);
localizationLinks.insert(std::make_pair(s->id(), landmarkLink2.merge(landmarkLink.inverse(), Link::kLandmark)));
} }
// Note that in localization mode, we don't re-optimize the graph // Note that in localization mode, we don't re-optimize the graph
@@ -4488,13 +4481,21 @@ bool Rtabmap::computePath(int targetNode, bool global)
} }
else else
{ {
if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0) if(_lastLocalizationPose.isNull() || _optimizedPoses.empty())
{ {
UWARN("Last localization pose is null... cannot compute a path"); UWARN("Last localization pose is null... cannot compute a path");
return false; return false;
} }
if(_optimizedPoses.begin()->first < 0)
{
std::map<int, Transform> poses(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
currentNode = graph::findNearestNode(poses, _lastLocalizationPose);
}
else
{
currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose); currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
} }
}
if(currentNode && targetNode) if(currentNode && targetNode)
{ {
std::list<std::pair<int, Transform> > path = graph::computePath( std::list<std::pair<int, Transform> > path = graph::computePath(
@@ -4637,13 +4638,21 @@ bool Rtabmap::computePath(const Transform & targetPose, float tolerance)
} }
else else
{ {
if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0) if(_lastLocalizationPose.isNull() || _optimizedPoses.empty())
{ {
UWARN("Last localization pose is null... cannot compute a path"); UWARN("Last localization pose is null... cannot compute a path");
return false; return false;
} }
if(_optimizedPoses.begin()->first < 0)
{
std::map<int, Transform> poses(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
currentNode = graph::findNearestNode(poses, _lastLocalizationPose);
}
else
{
currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose); currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
} }
}
int nearestId; int nearestId;
if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < tolerance) if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < tolerance)