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

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

6
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -515,8 +515,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(isLandmarkWithRotation.at(key))
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
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
{
@@ -617,8 +618,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(isLandmarkWithRotation.at(key))
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
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
{