diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 76fd1bfc7..58e98ebfa 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -955,7 +955,9 @@ 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)); + // 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,7 +1003,9 @@ 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)); + // 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); }