From 3cb73ccf35ebc5e6ca8e93b41d6eb9ca37eb6072 Mon Sep 17 00:00:00 2001 From: Matias Mattamala Date: Sun, 23 Oct 2022 16:57:06 +0100 Subject: [PATCH] Fix in writeG2o when writing Pose3 measurement --- gtsam/slam/dataset.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 65bddccee6..511cbd8390 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -722,11 +722,12 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w(); + // g2o's EDGE_SE3:QUAT stores information/precision of Pose3 in t,R order, unlike GTSAM: Matrix6 InfoG2o = I_6x6; InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation InfoG2o.block<3, 3>(3, 3) = Info.block<3, 3>(0, 0); // cov rotation - InfoG2o.block<3, 3>(0, 3) = Info.block<3, 3>(0, 3); // off diagonal - InfoG2o.block<3, 3>(3, 0) = Info.block<3, 3>(3, 0); // off diagonal + InfoG2o.block<3, 3>(0, 3) = Info.block<3, 3>(3, 0); // off diagonal R,t -> t,R + InfoG2o.block<3, 3>(3, 0) = Info.block<3, 3>(0, 3); // off diagonal t,R -> R,t for (size_t i = 0; i < 6; i++) { for (size_t j = i; j < 6; j++) { @@ -853,12 +854,12 @@ template <> struct ParseMeasurement { if (sampler) T12 = T12.retract(sampler->sample()); - // EDGE_SE3:QUAT stores information/precision in t,R order, unlike GTSAM: + // g2o's EDGE_SE3:QUAT stores information/precision of Pose3 in t,R order, unlike GTSAM: Matrix6 mgtsam; mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // info rotation mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // info translation - mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(0, 3); // off diagonal swap - mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(3, 0); // off diagonal swap + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(0, 3); // off diagonal g2o t,R -> GTSAM R,t + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(3, 0); // off diagonal g2o R,t -> GTSAM t,R SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); return BinaryMeasurement(