From f1f0c39be80c1a5a175d81a1d6caff5dbd81c024 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 8 Mar 2019 17:31:05 -0500 Subject: [PATCH] Fixed localization done in 3d instead of 2d when Reg/Force3DoF was true --- corelib/src/Rtabmap.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 4fc7925b..81dd17d4 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -2500,6 +2500,11 @@ bool Rtabmap::process( Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first); Transform mapCorrectionInv = _mapCorrection.inverse(); Transform u = signature->getPose() * localizationLinks.begin()->second.transform(); + if(_graphOptimizer->isSlam2d()) + { + // in case of 3d landmarks, transform constraint to 2D + u = u.to3DoF(); + } Transform up = u * oldPose.inverse(); for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { @@ -2509,7 +2514,13 @@ bool Rtabmap::process( } else { - _optimizedPoses.at(signature->id()) = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse(); + Transform newPose = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse(); + if(_graphOptimizer->isSlam2d()) + { + // in case of 3d landmarks, transform constraint to 2D + newPose = newPose.to3DoF(); + } + _optimizedPoses.at(signature->id()) = newPose; } localizationCovariance = localizationLinks.begin()->second.infMatrix().inv(); }