Skip to content

Commit

Permalink
add comments about tangent space and covariance matrix ordering
Browse files Browse the repository at this point in the history
  • Loading branch information
johnwlambert authored Jul 21, 2021
1 parent 5fee983 commit 28ecc33
Showing 1 changed file with 4 additions and 0 deletions.
4 changes: 4 additions & 0 deletions gtsam/sfm/ShonanAveraging.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -955,6 +955,8 @@ static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
"parseMeasurements<Rot2> 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<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
model);
Expand Down Expand Up @@ -1001,6 +1003,8 @@ static BinaryMeasurement<Rot3> convert(
"parseMeasurements<Rot3> 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<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
model);
Expand Down

0 comments on commit 28ecc33

Please sign in to comment.