12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #829 from borglab/shonan-covariance-fixes 

ShonanAveraging Covariance fixes
release/4.3a0
John Lambert 2021-07-22 13:40:08 -06:00 committed by GitHub
parent 838e74dbc8 28ecc3331b
commit 2b02cada1d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/sfm/ShonanAveraging.cpp
View File

@ -955,7 +955,9 @@ static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
"parseMeasurements<Rot2> can only convert Pose2 measurements " "parseMeasurements<Rot2> can only convert Pose2 measurements "
"with Gaussian noise models."); "with Gaussian noise models.");
const Matrix3 M = gaussian->covariance(); 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<Rot2>(f->key1(), f->key2(), f->measured().rotation(), return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
model); model);
} }
@ -1001,7 +1003,9 @@ static BinaryMeasurement<Rot3> convert(
"parseMeasurements<Rot3> can only convert Pose3 measurements " "parseMeasurements<Rot3> can only convert Pose3 measurements "
"with Gaussian noise models."); "with Gaussian noise models.");
const Matrix6 M = gaussian->covariance(); 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<Rot3>(f->key1(), f->key2(), f->measured().rotation(), return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
model); model);
} }