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

0.10.12: Added rtabmap::getLastLocalizationPose() method (to get last corrected pose either in mapping or localization mode)

Browse Source
This commit is contained in:
matlabbe
2015-11-25 15:45:27 -05:00
parent 10d683a38e
commit cd39a8c027
2 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+2 -2
View File
@@ -1097,7 +1097,7 @@ bool Rtabmap::process(
// Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // used in localization mode only (path planning)
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(signature->getLinks().size() == 1 &&
signature->getLinks().begin()->second.type() == Link::kNeighbor)
{
@@ -2266,7 +2266,7 @@ bool Rtabmap::process(
// Update map correction, it should be identify when optimizing from the last node
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update in case we switch to localization mode
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
{
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());