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

Fixed localization done in 3d instead of 2d when Reg/Force3DoF was true

Browse Source
This commit is contained in:
matlabbe
2019-03-08 17:31:05 -05:00
parent cf4db63226
commit f1f0c39be8
1 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/src/Rtabmap.cpp
View File

@@ -2500,6 +2500,11 @@ bool Rtabmap::process(
Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first); Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first);
Transform mapCorrectionInv = _mapCorrection.inverse(); Transform mapCorrectionInv = _mapCorrection.inverse();
Transform u = signature->getPose() * localizationLinks.begin()->second.transform(); 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(); Transform up = u * oldPose.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)
{ {
@@ -2509,7 +2514,13 @@ bool Rtabmap::process(
} }
else 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(); localizationCovariance = localizationLinks.begin()->second.infMatrix().inv();
} }