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

OptimizerGTSAM: fixed inverted landmark link transform on slam2d with rotation used

Browse Source
This commit is contained in:
matlabbe
2019-01-15 17:58:23 -05:00
parent 9da2c1918f
commit 95f48b694e
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -132,7 +132,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
} }
} }
UDEBUG("fill poses to gtsam..."); UDEBUG("fill poses to gtsam... rootId=%d", rootId);
gtsam::Values initialEstimate; gtsam::Values initialEstimate;
std::map<int, bool> isLandmarkWithRotation; std::map<int, bool> isLandmarkWithRotation;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -276,7 +276,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
} }
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model));
} }
else else
{ {