i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed not optimized map from first node if we switch RGBD/OptimizeFromGraphEnd parameter on and off while mapping.

Browse Source
This commit is contained in:
matlabbe
2016-01-14 12:44:18 -05:00
parent 6cc511cb80
commit 2ae1b84f85
1 changed files with 9 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/Rtabmap.cpp
View File

@@ -2059,6 +2059,15 @@ bool Rtabmap::process(
UINFO("Update map correction");
std::map<int, Transform> poses = _optimizedPoses;
// if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess
float normMapCorrection = _mapCorrection.getNormSquared(); // use distance for identity detection
if((normMapCorrection > 0.001f && _optimizeFromGraphEnd) ||
(normMapCorrection < 0.001f && !_optimizeFromGraphEnd))
{
poses.clear();
}
std::multimap<int, Link> constraints;
optimizeCurrentMap(signature->id(), false, poses, &constraints, &optimizationError, &optimizationIterations);