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

Fixed pose scan matching drift (with OptimizeFromGraphEnd=true)

Browse Source
This commit is contained in:
matlabbe
2015-09-14 09:40:28 -04:00
parent a45aa5d8f7
commit e290dfc111
1 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+4 -1
View File
@@ -1087,10 +1087,13 @@ bool Rtabmap::process(
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction. // parameters just changed state, we should put back all poses without map correction.
Transform u = guess.inverse() * t; Transform u = guess.inverse() * t;
std::map<int, Transform>::iterator iter = _optimizedPoses.find(oldId);
UASSERT(iter!=_optimizedPoses.end());
Transform up = iter->second * u * iter->second.inverse();
Transform mapCorrectionInv = _mapCorrection.inverse(); Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{ {
iter->second = mapCorrectionInv * iter->second * u; iter->second = mapCorrectionInv * up * iter->second;
} }
} }
} }