i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 09:07:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

OptimizerGTSAM: fixed landmarks not transformed back in 3D after 2D optimization (slam2d)

Browse Source
This commit is contained in:
matlabbe
2019-07-17 12:15:04 -04:00
parent 5222506578
commit 3558640407
1 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/optimizer/OptimizerGTSAM.cpp
+4 -2
View File
@@ -515,8 +515,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{ {
if(isLandmarkWithRotation.at(key)) if(isLandmarkWithRotation.at(key))
{ {
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>(); gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta()))); tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
} }
else else
{ {
@@ -617,8 +618,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{ {
if(isLandmarkWithRotation.at(key)) if(isLandmarkWithRotation.at(key))
{ {
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>(); gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta()))); optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
} }
else else
{ {