From 749907180274dda393e92a1b5019f43bcd49819e Mon Sep 17 00:00:00 2001 From: Maurice Fallon Date: Sat, 22 Oct 2022 18:57:41 +0100 Subject: [PATCH 1/2] fix typo in g2o file format parsing --- gtsam/slam/dataset.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 71dd64dbb6..65bddccee6 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -853,12 +853,12 @@ template <> struct ParseMeasurement { if (sampler) T12 = T12.retract(sampler->sample()); - // EDGE_SE3:QUAT stores covariance in t,R order, unlike GTSAM: + // EDGE_SE3:QUAT stores information/precision in t,R order, unlike GTSAM: Matrix6 mgtsam; - mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation - mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation - mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal - mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal + 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 SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); return BinaryMeasurement( From 3cb73ccf35ebc5e6ca8e93b41d6eb9ca37eb6072 Mon Sep 17 00:00:00 2001 From: Matias Mattamala Date: Sun, 23 Oct 2022 16:57:06 +0100 Subject: [PATCH 2/2] 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(