diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index bb49a84d6..b6ec6a724 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -161,6 +161,16 @@ struct LieGroup { return v; } + // Functor for transforming covariance of Class + class TransformCovariance + { + private: + typename Class::Jacobian adjointMap_; + public: + explicit TransformCovariance(const Class &X) : adjointMap_{X.AdjointMap()} {} + typename Class::Jacobian operator()(const typename Class::Jacobian &covariance) + { return adjointMap_ * covariance * adjointMap_.transpose(); } + }; }; /// tag to assert a type is a Lie group diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 4343b5408..81f097bd9 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -845,71 +845,68 @@ TEST(Pose2 , TransformCovariance3) { // rotate { - auto cov = GenerateFullCovariance({{0.1, 0.3, 0.7}}); - auto cov_trans = RotateTranslate({{90.}}, {{}}, cov); + auto cov = FullCovarianceFromSigmas({0.1, 0.3, 0.7}); + auto transformed = Pose2::TransformCovariance{{0., 0., 90 * degree}}(cov); // interchange x and y axes - EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), cov(0, 0), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), cov(1, 1), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), cov(2, 2), 1e-9); - - EXPECT_DOUBLES_EQUAL(cov_trans(1, 0), -cov(1, 0), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 0), -cov(2, 1), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 1), cov(2, 0), 1e-9); + EXPECT(assert_equal( + Vector3{cov(1, 1), cov(0, 0), cov(2, 2)}, + Vector3{transformed.diagonal()})); + EXPECT(assert_equal( + Vector3{-cov(1, 0), -cov(2, 1), cov(2, 0)}, + Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)})); } // translate along x with uncertainty in x { - auto cov = GenerateOneVariableCovariance(0, 0.1); - auto cov_trans = RotateTranslate({{}}, {{20., 0.}}, cov); - EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), 0.1 * 0.1, 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), 0., 1e-9); - - EXPECT_DOUBLES_EQUAL(cov_trans(1, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 1), 0., 1e-9); + auto cov = SingleVariableCovarianceFromSigma(0, 0.1); + auto transformed = Pose2::TransformCovariance{{20., 0., 0.}}(cov); + // No change + EXPECT(assert_equal(cov, transformed)); } // translate along x with uncertainty in y { - auto cov = GenerateOneVariableCovariance(1, 0.1); - auto cov_trans = RotateTranslate({{}}, {{20., 0.}}, cov); - EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0.1 * 0.1, 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), 0., 1e-9); - - EXPECT_DOUBLES_EQUAL(cov_trans(1, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 1), 0., 1e-9); + auto cov = SingleVariableCovarianceFromSigma(1, 0.1); + auto transformed = Pose2::TransformCovariance{{20., 0., 0.}}(cov); + // No change + EXPECT(assert_equal(cov, transformed)); } // translate along x with uncertainty in theta { - auto cov = GenerateOneVariableCovariance(2, 0.1); - auto cov_trans = RotateTranslate({{}}, {{20., 0.}}, cov); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 1), -0.1 * 0.1 * 20., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0.1 * 0.1 * 20. * 20., 1e-9); + auto cov = SingleVariableCovarianceFromSigma(2, 0.1); + auto transformed = Pose2::TransformCovariance{{20., 0., 0.}}(cov); + EXPECT(assert_equal( + Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1}, + Vector3{transformed.diagonal()})); + EXPECT(assert_equal( + Vector3{0., 0., -0.1 * 0.1 * 20.}, + Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)})); } // rotate and translate along x with uncertainty in x { - auto cov = GenerateOneVariableCovariance(0, 0.1); - auto cov_trans = RotateTranslate({{90.}}, {{20., 0.}}, cov); - EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0.1 * 0.1, 1e-9); + auto cov = SingleVariableCovarianceFromSigma(0, 0.1); + auto transformed = Pose2::TransformCovariance{{20., 0., 90 * degree}}(cov); + // interchange x and y axes + EXPECT(assert_equal( + Vector3{cov(1, 1), cov(0, 0), cov(2, 2)}, + Vector3{transformed.diagonal()})); + EXPECT(assert_equal( + Vector3{Z_3x1}, + Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)})); } // rotate and translate along x with uncertainty in theta { - auto cov = GenerateOneVariableCovariance(2, 0.1); - auto cov_trans = RotateTranslate({{90.}}, {{20., 0.}}, cov); - EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), 0.1 * 0.1 * 20. * 20., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), 0.1 * 0.1, 1e-9); - - EXPECT_DOUBLES_EQUAL(cov_trans(1, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 1), -0.1 * 0.1 * 20., 1e-9); + auto cov = SingleVariableCovarianceFromSigma(2, 0.1); + auto transformed = Pose2::TransformCovariance{{20., 0., 90 * degree}}(cov); + EXPECT(assert_equal( + Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1}, + Vector3{transformed.diagonal()})); + EXPECT(assert_equal( + Vector3{0., 0., -0.1 * 0.1 * 20.}, + Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)})); } } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 97153238a..fc702d3b3 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -885,43 +885,41 @@ TEST(Pose3, TransformCovariance6MapTo2d) { // Create 3d scenarios that map to 2d configurations and compare with Pose2 results. using namespace test_pose_adjoint_map; - std::array s{{0.1, 0.3, 0.7}}; - std::array r{{31.}}; - std::array t{{1.1, 1.5}}; - auto cov2 = GenerateFullCovariance({{0.1, 0.3, 0.7}}); - auto cov2_trans = RotateTranslate(r, t, cov2); + Vector3 s2{0.1, 0.3, 0.7}; + Pose2 p2{1.1, 1.5, 31. * degree}; + auto cov2 = FullCovarianceFromSigmas(s2); + auto transformed2 = Pose2::TransformCovariance{p2}(cov2); - auto match_cov3_to_cov2 = [&](int r_axis, int spatial_axis0, int spatial_axis1, + auto match_cov3_to_cov2 = [&](int spatial_axis0, int spatial_axis1, int r_axis, const Pose2::Jacobian &cov2, const Pose3::Jacobian &cov3) -> void { - EXPECT_DOUBLES_EQUAL(cov2(0, 0), cov3(spatial_axis0, spatial_axis0), 1e-9); - EXPECT_DOUBLES_EQUAL(cov2(1, 1), cov3(spatial_axis1, spatial_axis1), 1e-9); - EXPECT_DOUBLES_EQUAL(cov2(2, 2), cov3(r_axis, r_axis), 1e-9); - - EXPECT_DOUBLES_EQUAL(cov2(1, 0), cov3(spatial_axis1, spatial_axis0), 1e-9); - EXPECT_DOUBLES_EQUAL(cov2(2, 1), cov3(r_axis, spatial_axis1), 1e-9); - EXPECT_DOUBLES_EQUAL(cov2(2, 0), cov3(r_axis, spatial_axis0), 1e-9); + EXPECT(assert_equal( + Vector3{cov2.diagonal()}, + Vector3{cov3(spatial_axis0, spatial_axis0), cov3(spatial_axis1, spatial_axis1), cov3(r_axis, r_axis)})); + EXPECT(assert_equal( + Vector3{cov2(1, 0), cov2(2, 0), cov2(2, 1)}, + Vector3{cov3(spatial_axis1, spatial_axis0), cov3(r_axis, spatial_axis0), cov3(r_axis, spatial_axis1)})); }; // rotate around x axis { - auto cov3 = GenerateFullCovariance({{s[2], 0., 0., 0., s[0], s[1]}}); - auto cov3_trans = RotateTranslate({{r[0], 0., 0.}}, {{0., t[0], t[1]}}, cov3); - match_cov3_to_cov2(0, 4, 5, cov2_trans, cov3_trans); + auto cov3 = FullCovarianceFromSigmas((Vector6{} << s2(2), 0., 0., 0., s2(0), s2(1)).finished()); + auto transformed3 = Pose3::TransformCovariance{{Rot3::RzRyRx(p2.theta(), 0., 0.), {0., p2.x(), p2.y()}}}(cov3); + match_cov3_to_cov2(4, 5, 0, transformed2, transformed3); } // rotate around y axis { - auto cov3 = GenerateFullCovariance({{0., s[2], 0., s[1], 0., s[0]}}); - auto cov3_trans = RotateTranslate({{0., r[0], 0.}}, {{t[1], 0., t[0]}}, cov3); - match_cov3_to_cov2(1, 5, 3, cov2_trans, cov3_trans); + auto cov3 = FullCovarianceFromSigmas((Vector6{} << 0., s2(2), 0., s2(1), 0., s2(0)).finished()); + auto transformed3 = Pose3::TransformCovariance{{Rot3::RzRyRx(0., p2.theta(), 0.), {p2.y(), 0., p2.x()}}}(cov3); + match_cov3_to_cov2(5, 3, 1, transformed2, transformed3); } // rotate around z axis { - auto cov3 = GenerateFullCovariance({{0., 0., s[2], s[0], s[1], 0.}}); - auto cov3_trans = RotateTranslate({{0., 0., r[0]}}, {{t[0], t[1], 0.}}, cov3); - match_cov3_to_cov2(2, 3, 4, cov2_trans, cov3_trans); + auto cov3 = FullCovarianceFromSigmas((Vector6{} << 0., 0., s2(2), s2(0), s2(1), 0.).finished()); + auto transformed3 = Pose3::TransformCovariance{{Rot3::RzRyRx(0., 0., p2.theta()), {p2.x(), p2.y(), 0.}}}(cov3); + match_cov3_to_cov2(3, 4, 2, transformed2, transformed3); } } @@ -933,54 +931,49 @@ TEST(Pose3, TransformCovariance6) { // rotate 90 around z axis and then 90 around y axis { - auto cov = GenerateFullCovariance({{0.1, 0.2, 0.3, 0.5, 0.7, 1.1}}); - auto cov_trans = RotateTranslate({{0., 90., 90.}}, {{}}, cov); + auto cov = FullCovarianceFromSigmas((Vector6{} << 0.1, 0.2, 0.3, 0.5, 0.7, 1.1).finished()); + auto transformed = Pose3::TransformCovariance{{Rot3::RzRyRx(0., 90 * degree, 90 * degree), {0., 0., 0.}}}(cov); // x from y, y from z, z from x - EXPECT_DOUBLES_EQUAL(cov_trans(0, 0), cov(1, 1), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), cov(2, 2), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), cov(0, 0), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(3, 3), cov(4, 4), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(4, 4), cov(5, 5), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(5, 5), cov(3, 3), 1e-9); - + EXPECT(assert_equal( + (Vector6{} << cov(1, 1), cov(2, 2), cov(0, 0), cov(4, 4), cov(5, 5), cov(3, 3)).finished(), + Vector6{transformed.diagonal()})); // Both the x and z axes are pointing in the negative direction. - EXPECT_DOUBLES_EQUAL(cov_trans(1, 0), -cov(2, 1), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 0), cov(0, 1), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(3, 0), cov(4, 1), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(4, 0), -cov(5, 1), 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(5, 0), cov(3, 1), 1e-9); + EXPECT(assert_equal( + (Vector5{} << -cov(2, 1), cov(0, 1), cov(4, 1), -cov(5, 1), cov(3, 1)).finished(), + (Vector5{} << transformed(1, 0), transformed(2, 0), transformed(3, 0), + transformed(4, 0), transformed(5, 0)).finished())); } // translate along the x axis with uncertainty in roty and rotz { - auto cov = GenerateTwoVariableCovariance(1, 2, 0.7, 0.3); - auto cov_trans = RotateTranslate({{}}, {{20., 0., 0.}}, cov); - // The variance in roty and rotz causes off-diagonal covariances - EXPECT_DOUBLES_EQUAL(cov_trans(5, 1), 0.7 * 0.7 * 20., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(5, 5), 0.7 * 0.7 * 20. * 20., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(4, 2), -0.3 * 0.3 * 20., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(4, 4), 0.3 * 0.3 * 20. * 20., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(4, 1), -0.3 * 0.7 * 20., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(5, 2), 0.3 * 0.7 * 20., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(5, 4), -0.3 * 0.7 * 20. * 20., 1e-9); + auto cov = TwoVariableCovarianceFromSigmas(1, 2, 0.7, 0.3); + auto transformed = Pose3::TransformCovariance{{Rot3::RzRyRx(0., 0., 0.), {20., 0., 0.}}}(cov); + // The uncertainty in roty and rotz causes off-diagonal covariances + EXPECT(assert_equal(0.7 * 0.7 * 20., transformed(5, 1))); + EXPECT(assert_equal(0.7 * 0.7 * 20. * 20., transformed(5, 5))); + EXPECT(assert_equal(-0.3 * 0.3 * 20., transformed(4, 2))); + EXPECT(assert_equal(0.3 * 0.3 * 20. * 20., transformed(4, 4))); + EXPECT(assert_equal(-0.3 * 0.7 * 20., transformed(4, 1))); + EXPECT(assert_equal(0.3 * 0.7 * 20., transformed(5, 2))); + EXPECT(assert_equal(-0.3 * 0.7 * 20. * 20., transformed(5, 4))); } // rotate around x axis and translate along the x axis with uncertainty in rotx { - auto cov = GenerateOneVariableCovariance(0, 0.1); - auto cov_trans = RotateTranslate({{90., 0., 0.}}, {{20., 0., 0.}}, cov); - EXPECT_DOUBLES_EQUAL(cov_trans(1, 0), 0., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(2, 0), 0., 1e-9); + auto cov = SingleVariableCovarianceFromSigma(0, 0.1); + auto transformed = Pose3::TransformCovariance{{Rot3::RzRyRx(90 * degree, 0., 0.), {20., 0., 0.}}}(cov); + // No change + EXPECT(assert_equal(cov, transformed)); } // rotate around x axis and translate along the x axis with uncertainty in roty { - auto cov = GenerateOneVariableCovariance(1, 0.1); - auto cov_trans = RotateTranslate({{90., 0., 0.}}, {{20., 0., 0.}}, cov); - // interchange the y and z axes. - EXPECT_DOUBLES_EQUAL(cov_trans(2, 2), 0.1 * 0.1, 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(4, 2), -0.1 * 0.1 * 20., 1e-9); - EXPECT_DOUBLES_EQUAL(cov_trans(4, 4), 0.1 * 0.1 * 20. * 20., 1e-9); + auto cov = SingleVariableCovarianceFromSigma(1, 0.1); + auto transformed = Pose3::TransformCovariance{{Rot3::RzRyRx(90 * degree, 0., 0.), {20., 0., 0.}}}(cov); + // Uncertainty is spread to other dimensions. + EXPECT(assert_equal( + (Vector6{} << 0., 0., 0.1 * 0.1, 0., 0.1 * 0.1 * 20. * 20., 0.).finished(), + Vector6{transformed.diagonal()})); } } diff --git a/gtsam/geometry/tests/testPoseAdjointMap.h b/gtsam/geometry/tests/testPoseAdjointMap.h index 1263a807c..1d006c3d9 100644 --- a/gtsam/geometry/tests/testPoseAdjointMap.h +++ b/gtsam/geometry/tests/testPoseAdjointMap.h @@ -25,30 +25,30 @@ namespace test_pose_adjoint_map { const double degree = M_PI / 180; - // Create a covariance matrix for type T. Use sigma_values^2 on the diagonal - // and fill in non-diagonal entries with correlation coefficient of 1. Note: - // a covariance matrix for T has the same dimensions as a Jacobian for T. + // Return a covariance matrix for type T with non-zero values for every element. + // Use sigma_values^2 on the diagonal and fill in non-diagonal entries with + // correlation coefficient of 1. Note: a covariance matrix for T has the same + // dimensions as a Jacobian for T, the returned matrix is not a Jacobian. template - typename T::Jacobian GenerateFullCovariance( - std::array sigma_values) + typename T::Jacobian FullCovarianceFromSigmas( + const typename T::TangentVector &sigmas) { - typename T::TangentVector sigmas(&sigma_values.front()); - return typename T::Jacobian{sigmas * sigmas.transpose()}; + return sigmas * sigmas.transpose(); } - // Create a covariance matrix with one non-zero element on the diagonal. + // Return a covariance matrix with one non-zero element on the diagonal. template - typename T::Jacobian GenerateOneVariableCovariance(int idx, double sigma) + typename T::Jacobian SingleVariableCovarianceFromSigma(int idx, double sigma) { typename T::Jacobian cov = T::Jacobian::Zero(); cov(idx, idx) = sigma * sigma; return cov; } - // Create a covariance matrix with two non-zero elements on the diagonal with - // a correlation of 1.0 + // Return a covariance matrix with two non-zero elements on the diagonal and + // a correlation of 1.0 between the two variables. template - typename T::Jacobian GenerateTwoVariableCovariance(int idx0, int idx1, double sigma0, double sigma1) + typename T::Jacobian TwoVariableCovarianceFromSigmas(int idx0, int idx1, double sigma0, double sigma1) { typename T::Jacobian cov = T::Jacobian::Zero(); cov(idx0, idx0) = sigma0 * sigma0; @@ -56,32 +56,4 @@ namespace test_pose_adjoint_map cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1; return cov; } - - // Overloaded function to create a Rot2 from one angle. - Rot2 RotFromArray(const std::array &r) - { - return Rot2{r[0] * degree}; - } - - // Overloaded function to create a Rot3 from three angles. - Rot3 RotFromArray(const std::array &r) - { - return Rot3::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree); - } - - // Transform a covariance matrix with a rotation and a translation - template - typename Pose::Jacobian RotateTranslate( - std::array r, - std::array t, - const typename Pose::Jacobian &cov) - { - // Construct a pose object - typename Pose::Rotation rot{RotFromArray(r)}; - Pose wTb{rot, typename Pose::Translation{&t.front()}}; - - // transform the covariance with the AdjointMap - auto adjointMap = wTb.AdjointMap(); - return adjointMap * cov * adjointMap.transpose(); - } } \ No newline at end of file