From 5fee983ff102bda87833af53c10ba7e972eca583 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 21 Jul 2021 10:04:05 -0600 Subject: [PATCH 1/2] use upper 3x3 sub-block of covariance matrix for converting BetweenFactor to BinaryMeasurement, and use Isotropic in ShonanAveraging2 --- gtsam/sfm/ShonanAveraging.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 76fd1bfc7..a5b6a8d9a 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -955,7 +955,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); const Matrix3 M = gaussian->covariance(); - auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2)); + auto model = noiseModel::Isotropic::Variance(1, M(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); } @@ -1001,7 +1001,7 @@ static BinaryMeasurement convert( "parseMeasurements can only convert Pose3 measurements " "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); - auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3)); + auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); } From 28ecc3331bda0bf49f94fb52630efe77926a5024 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 21 Jul 2021 10:27:31 -0600 Subject: [PATCH 2/2] add comments about tangent space and covariance matrix ordering --- gtsam/sfm/ShonanAveraging.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index a5b6a8d9a..58e98ebfa 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -955,6 +955,8 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); const Matrix3 M = gaussian->covariance(); + // the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance + // because the tangent space of Pose2 is ordered as (vx, vy, w) auto model = noiseModel::Isotropic::Variance(1, M(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); @@ -1001,6 +1003,8 @@ static BinaryMeasurement convert( "parseMeasurements can only convert Pose3 measurements " "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); + // the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance + // because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model);