From 981f1851c9fcafc7980f5d930662317f5f182299 Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Thu, 12 Dec 2019 22:55:55 -0800 Subject: [PATCH 1/7] add rotate Pose2 and Pose3 covariance tests --- gtsam/geometry/tests/testPose2.cpp | 131 +++++++++++++++++++++ gtsam/geometry/tests/testPose3.cpp | 178 +++++++++++++++++++++++++++++ 2 files changed, 309 insertions(+) diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index b8b3fc52c..6fc850fdd 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -835,6 +835,137 @@ TEST(Pose2 , ChartDerivatives) { CHECK_CHART_DERIVATIVES(T2,T1); } +//****************************************************************************** +namespace transform_covariance3 { + const double degree = M_PI / 180; + + template + static typename TPose::Jacobian generate_full_covariance( + std::array sigma_values) + { + typename TPose::TangentVector sigmas(&sigma_values.front()); + return typename TPose::Jacobian{sigmas * sigmas.transpose()}; + } + + template + static typename TPose::Jacobian generate_one_variable_covariance(int idx, double sigma) + { + typename TPose::Jacobian cov = TPose::Jacobian::Zero(); + cov(idx, idx) = sigma * sigma; + return cov; + } + + template + static typename TPose::Jacobian transform_covariance( + const TPose &wTb, + const typename TPose::Jacobian &cov) + { + // transform the covariance with the AdjointMap + auto adjointMap = wTb.AdjointMap(); + return adjointMap * cov * adjointMap.transpose(); + } + + static typename Pose2::Jacobian rotate_only( + const std::array r, + const typename Pose2::Jacobian &cov) + { + // Create a rotation only transform + Pose2 wTb{Pose2::Rotation{r[0] * degree}, Pose2::Translation{}}; + return transform_covariance(wTb, cov); + } + + static Pose2::Jacobian translate_only( + const std::array t, + const Pose2::Jacobian &cov) + { + // Create a translation only transform + Pose2 wTb{Pose2::Rotation{}, Pose2::Translation{t[0], t[1]}}; + return transform_covariance(wTb, cov); + } + + static Pose2::Jacobian rotate_translate( + const std::array r, + const std::array t, + const Pose2::Jacobian &cov) + { + // Create a translation only transform + Pose2 wTb{Pose2::Rotation{r[0] * degree}, Pose2::Translation{t[0], t[1]}}; + return transform_covariance(wTb, cov); + } +} +TEST(Pose2 , TransformCovariance3) { + using namespace transform_covariance3; + + // rotate + { + auto cov = generate_full_covariance({0.1, 0.3, 0.7}); + auto cov_trans = rotate_only({90.}, 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); + } + + // translate along x with uncertainty in x + { + auto cov = generate_one_variable_covariance(0, 0.1); + auto cov_trans = translate_only({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); + } + + // translate along x with uncertainty in y + { + auto cov = generate_one_variable_covariance(1, 0.1); + auto cov_trans = translate_only({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); + } + + // translate along x with uncertainty in theta + { + auto cov = generate_one_variable_covariance(2, 0.1); + auto cov_trans = translate_only({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); + } + + // rotate and translate along x with uncertainty in x + { + auto cov = generate_one_variable_covariance(0, 0.1); + auto cov_trans = rotate_translate({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); + } + + // rotate and translate along x with uncertainty in theta + { + auto cov = generate_one_variable_covariance(2, 0.1); + auto cov_trans = rotate_translate({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); + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 0b2f59773..66f67f7c5 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -878,6 +878,184 @@ TEST(Pose3 , ChartDerivatives) { } } +//****************************************************************************** +namespace transform_covariance6 { + const double degree = M_PI / 180; + + template + static typename TPose::Jacobian generate_full_covariance( + std::array sigma_values) + { + typename TPose::TangentVector sigmas(&sigma_values.front()); + return typename TPose::Jacobian{sigmas * sigmas.transpose()}; + } + + template + static typename TPose::Jacobian generate_one_variable_covariance(int idx, double sigma) + { + typename TPose::Jacobian cov = TPose::Jacobian::Zero(); + cov(idx, idx) = sigma * sigma; + return cov; + } + + template + static typename TPose::Jacobian generate_two_variable_covariance(int idx0, int idx1, double sigma0, double sigma1) + { + typename TPose::Jacobian cov = TPose::Jacobian::Zero(); + cov(idx0, idx0) = sigma0 * sigma0; + cov(idx1, idx1) = sigma1 * sigma1; + cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1; + return cov; + } + + template + static typename TPose::Jacobian transform_covariance( + const TPose &wTb, + const typename TPose::Jacobian &cov) + { + // transform the covariance with the AdjointMap + auto adjointMap = wTb.AdjointMap(); + return adjointMap * cov * adjointMap.transpose(); + } + + static Pose2::Jacobian rotate_translate( + const std::array r, + const std::array t, + const Pose2::Jacobian &cov) + { + // Create a translation only transform + Pose2 wTb{Pose2::Rotation{r[0] * degree}, Pose2::Translation{t[0], t[1]}}; + return transform_covariance(wTb, cov); + } + + static typename Pose3::Jacobian rotate_only( + const std::array r, + const typename Pose3::Jacobian &cov) + { + // Create a rotation only transform + Pose3 wTb{Pose3::Rotation::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree), + Pose3::Translation{}}; + return transform_covariance(wTb, cov); + } + + static Pose3::Jacobian translate_only( + const std::array t, + const Pose3::Jacobian &cov) + { + // Create a translation only transform + Pose3 wTb{Pose3::Rotation{}, Pose3::Translation{t[0], t[1], t[2]}}; + return transform_covariance(wTb, cov); + } + + static Pose3::Jacobian rotate_translate( + const std::array r, + const std::array t, + const Pose3::Jacobian &cov) + { + // Create a translation only transform + Pose3 wTb{Pose3::Rotation::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree), + Pose3::Translation{t[0], t[1], t[2]}}; + return transform_covariance(wTb, cov); + } +} +TEST(Pose3, TransformCovariance6MapTo2d) { + using namespace transform_covariance6; + std::array s{{0.1, 0.3, 0.7}}; + std::array r{{31.}}; + std::array t{{1.1, 1.5}}; + auto cov2 = generate_full_covariance({0.1, 0.3, 0.7}); + auto cov2_trans = rotate_translate(r, t, cov2); + + auto match_cov3_to_cov2 = [&](int r_axis, int spatial_axis0, int spatial_axis1, + 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); + }; + + // rotate around x axis + { + auto cov3 = generate_full_covariance({s[2], 0., 0., 0., s[0], s[1]}); + auto cov3_trans = rotate_translate({r[0], 0., 0.}, {0., t[0], t[1]}, cov3); + match_cov3_to_cov2(0, 4, 5, cov2_trans, cov3_trans); + } + + // rotate around y axis + { + auto cov3 = generate_full_covariance({0., s[2], 0., s[1], 0., s[0]}); + auto cov3_trans = rotate_translate({0., r[0], 0.}, {t[1], 0., t[0]}, cov3); + match_cov3_to_cov2(1, 5, 3, cov2_trans, cov3_trans); + } + + // rotate around z axis + { + auto cov3 = generate_full_covariance({0., 0., s[2], s[0], s[1], 0.}); + auto cov3_trans = rotate_translate({0., 0., r[0]}, {t[0], t[1], 0.}, cov3); + match_cov3_to_cov2(2, 3, 4, cov2_trans, cov3_trans); + } +} + +/* ************************************************************************* */ +TEST(Pose3, TransformCovariance6) { + using namespace transform_covariance6; + // rotate 90 around z axis and then 90 around y axis + { + auto cov = generate_full_covariance({0.1, 0.2, 0.3, 0.5, 0.7, 1.1}); + auto cov_trans = rotate_only({0., 90., 90.}, 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); + + // 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); + } + + // translate along the x axis with uncertainty in roty and rotz + { + auto cov = generate_two_variable_covariance(1, 2, 0.7, 0.3); + auto cov_trans = translate_only({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); + } + + // rotate around x axis and translate along the x axis with uncertainty in rotx + { + auto cov = generate_one_variable_covariance(0, 0.1); + auto cov_trans = rotate_translate({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); + } + + // rotate around x axis and translate along the x axis with uncertainty in roty + { + auto cov = generate_one_variable_covariance(1, 0.1); + auto cov_trans = rotate_translate({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); + } +} + /* ************************************************************************* */ TEST(Pose3, interpolate) { EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0))); From 323a9ab9f234cbba87687607945ac0e8628853d7 Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Fri, 13 Dec 2019 07:57:43 -0800 Subject: [PATCH 2/7] a few comments --- gtsam/geometry/tests/testPose2.cpp | 2 ++ gtsam/geometry/tests/testPose3.cpp | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 6fc850fdd..a98c4472f 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -894,6 +894,8 @@ namespace transform_covariance3 { } } TEST(Pose2 , TransformCovariance3) { + // Use simple covariance matrices and transforms to create tests that can be + // validated with simple computations. using namespace transform_covariance3; // rotate diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 66f67f7c5..b7f5a553c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -959,6 +959,7 @@ namespace transform_covariance6 { } } TEST(Pose3, TransformCovariance6MapTo2d) { + // Create 3d scenarios that map to 2d configurations and compare with Pose2 results. using namespace transform_covariance6; std::array s{{0.1, 0.3, 0.7}}; std::array r{{31.}}; @@ -1002,7 +1003,10 @@ TEST(Pose3, TransformCovariance6MapTo2d) { /* ************************************************************************* */ TEST(Pose3, TransformCovariance6) { + // Use simple covariance matrices and transforms to create tests that can be + // validated with simple computations. using namespace transform_covariance6; + // rotate 90 around z axis and then 90 around y axis { auto cov = generate_full_covariance({0.1, 0.2, 0.3, 0.5, 0.7, 1.1}); From 46eb35ecffeac3f883238067693450ce88d2c7ca Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Sun, 15 Dec 2019 20:17:56 -0800 Subject: [PATCH 3/7] Attempt to fix clang compile warnings and execution failures. Change function names. Add a few comments. --- gtsam/geometry/tests/testPose2.cpp | 100 +++++++++++------------ gtsam/geometry/tests/testPose3.cpp | 123 +++++++++++++---------------- 2 files changed, 104 insertions(+), 119 deletions(-) diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index a98c4472f..fec3fe426 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -839,59 +839,59 @@ TEST(Pose2 , ChartDerivatives) { namespace transform_covariance3 { const double degree = M_PI / 180; - template - static typename TPose::Jacobian generate_full_covariance( - std::array sigma_values) + // Create a covariance matrix for type T. Use sigma_values^2 on the diagonal + // and fill in non-diagonal entries with a correlation coefficient of 1. Note: + // a covariance matrix for T has the same dimensions as a Jacobian for T. + template + static typename T::Jacobian GenerateFullCovariance( + std::array sigma_values) { - typename TPose::TangentVector sigmas(&sigma_values.front()); - return typename TPose::Jacobian{sigmas * sigmas.transpose()}; + typename T::TangentVector sigmas(&sigma_values.front()); + return typename T::Jacobian{sigmas * sigmas.transpose()}; } - template - static typename TPose::Jacobian generate_one_variable_covariance(int idx, double sigma) + // Create a covariance matrix with one non-zero element on the diagonal. + template + static typename T::Jacobian GenerateOneVariableCovariance(int idx, double sigma) { - typename TPose::Jacobian cov = TPose::Jacobian::Zero(); + 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 + template + static typename T::Jacobian GenerateTwoVariableCovariance(int idx0, int idx1, double sigma0, double sigma1) + { + typename T::Jacobian cov = T::Jacobian::Zero(); + cov(idx0, idx0) = sigma0 * sigma0; + cov(idx1, idx1) = sigma1 * sigma1; + cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1; + return cov; + } + + // Overloaded function to create a Rot2 from one angle. + static Rot2 RotFromArray(const std::array &r) + { + return Rot2{r[0] * degree}; + } + + // Transform a covariance matrix with a rotation and a translation template - static typename TPose::Jacobian transform_covariance( - const TPose &wTb, + static typename TPose::Jacobian RotateTranslate( + std::array r, + std::array t, const typename TPose::Jacobian &cov) { + // Construct a pose object + typename TPose::Rotation rot{RotFromArray(r)}; + TPose wTb{rot, typename TPose::Translation{&t.front()}}; + // transform the covariance with the AdjointMap auto adjointMap = wTb.AdjointMap(); return adjointMap * cov * adjointMap.transpose(); } - - static typename Pose2::Jacobian rotate_only( - const std::array r, - const typename Pose2::Jacobian &cov) - { - // Create a rotation only transform - Pose2 wTb{Pose2::Rotation{r[0] * degree}, Pose2::Translation{}}; - return transform_covariance(wTb, cov); - } - - static Pose2::Jacobian translate_only( - const std::array t, - const Pose2::Jacobian &cov) - { - // Create a translation only transform - Pose2 wTb{Pose2::Rotation{}, Pose2::Translation{t[0], t[1]}}; - return transform_covariance(wTb, cov); - } - - static Pose2::Jacobian rotate_translate( - const std::array r, - const std::array t, - const Pose2::Jacobian &cov) - { - // Create a translation only transform - Pose2 wTb{Pose2::Rotation{r[0] * degree}, Pose2::Translation{t[0], t[1]}}; - return transform_covariance(wTb, cov); - } } TEST(Pose2 , TransformCovariance3) { // Use simple covariance matrices and transforms to create tests that can be @@ -900,8 +900,8 @@ TEST(Pose2 , TransformCovariance3) { // rotate { - auto cov = generate_full_covariance({0.1, 0.3, 0.7}); - auto cov_trans = rotate_only({90.}, cov); + auto cov = GenerateFullCovariance({0.1, 0.3, 0.7}); + auto cov_trans = RotateTranslate({{90.}}, {{}}, 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); @@ -914,8 +914,8 @@ TEST(Pose2 , TransformCovariance3) { // translate along x with uncertainty in x { - auto cov = generate_one_variable_covariance(0, 0.1); - auto cov_trans = translate_only({20., 0.}, cov); + 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); @@ -927,8 +927,8 @@ TEST(Pose2 , TransformCovariance3) { // translate along x with uncertainty in y { - auto cov = generate_one_variable_covariance(1, 0.1); - auto cov_trans = translate_only({20., 0.}, cov); + 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); @@ -940,24 +940,24 @@ TEST(Pose2 , TransformCovariance3) { // translate along x with uncertainty in theta { - auto cov = generate_one_variable_covariance(2, 0.1); - auto cov_trans = translate_only({20., 0.}, cov); + 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); } // rotate and translate along x with uncertainty in x { - auto cov = generate_one_variable_covariance(0, 0.1); - auto cov_trans = rotate_translate({90.}, {20., 0.}, cov); + 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); } // rotate and translate along x with uncertainty in theta { - auto cov = generate_one_variable_covariance(2, 0.1); - auto cov_trans = rotate_translate({90.}, {20., 0.}, cov); + 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); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index b7f5a553c..14abe3d8c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -882,90 +882,75 @@ TEST(Pose3 , ChartDerivatives) { namespace transform_covariance6 { const double degree = M_PI / 180; - template - static typename TPose::Jacobian generate_full_covariance( - std::array sigma_values) + // Create a covariance matrix for type T. Use sigma_values^2 on the diagonal + // and fill in non-diagonal entries with a correlation coefficient of 1. Note: + // a covariance matrix for T has the same dimensions as a Jacobian for T. + template + static typename T::Jacobian GenerateFullCovariance( + std::array sigma_values) { - typename TPose::TangentVector sigmas(&sigma_values.front()); - return typename TPose::Jacobian{sigmas * sigmas.transpose()}; + typename T::TangentVector sigmas(&sigma_values.front()); + return typename T::Jacobian{sigmas * sigmas.transpose()}; } - template - static typename TPose::Jacobian generate_one_variable_covariance(int idx, double sigma) + // Create a covariance matrix with one non-zero element on the diagonal. + template + static typename T::Jacobian GenerateOneVariableCovariance(int idx, double sigma) { - typename TPose::Jacobian cov = TPose::Jacobian::Zero(); + typename T::Jacobian cov = T::Jacobian::Zero(); cov(idx, idx) = sigma * sigma; return cov; } - template - static typename TPose::Jacobian generate_two_variable_covariance(int idx0, int idx1, double sigma0, double sigma1) + // Create a covariance matrix with two non-zero elements on the diagonal with + // a correlation of 1.0 + template + static typename T::Jacobian GenerateTwoVariableCovariance(int idx0, int idx1, double sigma0, double sigma1) { - typename TPose::Jacobian cov = TPose::Jacobian::Zero(); + typename T::Jacobian cov = T::Jacobian::Zero(); cov(idx0, idx0) = sigma0 * sigma0; cov(idx1, idx1) = sigma1 * sigma1; cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1; return cov; } + // Overloaded function to create a Rot2 from one angle. + static Rot2 RotFromArray(const std::array &r) + { + return Rot2{r[0] * degree}; + } + + // Overloaded function to create a Rot3 from three angles. + static 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 - static typename TPose::Jacobian transform_covariance( - const TPose &wTb, + static typename TPose::Jacobian RotateTranslate( + std::array r, + std::array t, const typename TPose::Jacobian &cov) { + // Construct a pose object + typename TPose::Rotation rot{RotFromArray(r)}; + TPose wTb{rot, typename TPose::Translation{&t.front()}}; + // transform the covariance with the AdjointMap auto adjointMap = wTb.AdjointMap(); return adjointMap * cov * adjointMap.transpose(); } - - static Pose2::Jacobian rotate_translate( - const std::array r, - const std::array t, - const Pose2::Jacobian &cov) - { - // Create a translation only transform - Pose2 wTb{Pose2::Rotation{r[0] * degree}, Pose2::Translation{t[0], t[1]}}; - return transform_covariance(wTb, cov); - } - - static typename Pose3::Jacobian rotate_only( - const std::array r, - const typename Pose3::Jacobian &cov) - { - // Create a rotation only transform - Pose3 wTb{Pose3::Rotation::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree), - Pose3::Translation{}}; - return transform_covariance(wTb, cov); - } - - static Pose3::Jacobian translate_only( - const std::array t, - const Pose3::Jacobian &cov) - { - // Create a translation only transform - Pose3 wTb{Pose3::Rotation{}, Pose3::Translation{t[0], t[1], t[2]}}; - return transform_covariance(wTb, cov); - } - - static Pose3::Jacobian rotate_translate( - const std::array r, - const std::array t, - const Pose3::Jacobian &cov) - { - // Create a translation only transform - Pose3 wTb{Pose3::Rotation::RzRyRx(r[0] * degree, r[1] * degree, r[2] * degree), - Pose3::Translation{t[0], t[1], t[2]}}; - return transform_covariance(wTb, cov); - } } TEST(Pose3, TransformCovariance6MapTo2d) { // Create 3d scenarios that map to 2d configurations and compare with Pose2 results. using namespace transform_covariance6; + std::array s{{0.1, 0.3, 0.7}}; std::array r{{31.}}; std::array t{{1.1, 1.5}}; - auto cov2 = generate_full_covariance({0.1, 0.3, 0.7}); - auto cov2_trans = rotate_translate(r, t, cov2); + auto cov2 = GenerateFullCovariance({{0.1, 0.3, 0.7}}); + auto cov2_trans = RotateTranslate(r, t, cov2); auto match_cov3_to_cov2 = [&](int r_axis, int spatial_axis0, int spatial_axis1, const Pose2::Jacobian &cov2, const Pose3::Jacobian &cov3) -> void @@ -981,22 +966,22 @@ TEST(Pose3, TransformCovariance6MapTo2d) { // rotate around x axis { - auto cov3 = generate_full_covariance({s[2], 0., 0., 0., s[0], s[1]}); - auto cov3_trans = rotate_translate({r[0], 0., 0.}, {0., t[0], t[1]}, cov3); + 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); } // rotate around y axis { - auto cov3 = generate_full_covariance({0., s[2], 0., s[1], 0., s[0]}); - auto cov3_trans = rotate_translate({0., r[0], 0.}, {t[1], 0., t[0]}, cov3); + 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); } // rotate around z axis { - auto cov3 = generate_full_covariance({0., 0., s[2], s[0], s[1], 0.}); - auto cov3_trans = rotate_translate({0., 0., r[0]}, {t[0], t[1], 0.}, cov3); + 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); } } @@ -1009,8 +994,8 @@ TEST(Pose3, TransformCovariance6) { // rotate 90 around z axis and then 90 around y axis { - auto cov = generate_full_covariance({0.1, 0.2, 0.3, 0.5, 0.7, 1.1}); - auto cov_trans = rotate_only({0., 90., 90.}, cov); + auto cov = GenerateFullCovariance({{0.1, 0.2, 0.3, 0.5, 0.7, 1.1}}); + auto cov_trans = RotateTranslate({{0., 90., 90.}}, {{}}, 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); @@ -1029,8 +1014,8 @@ TEST(Pose3, TransformCovariance6) { // translate along the x axis with uncertainty in roty and rotz { - auto cov = generate_two_variable_covariance(1, 2, 0.7, 0.3); - auto cov_trans = translate_only({20., 0., 0.}, cov); + 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); @@ -1043,16 +1028,16 @@ TEST(Pose3, TransformCovariance6) { // rotate around x axis and translate along the x axis with uncertainty in rotx { - auto cov = generate_one_variable_covariance(0, 0.1); - auto cov_trans = rotate_translate({90., 0., 0.}, {20., 0., 0.}, cov); + 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); } // rotate around x axis and translate along the x axis with uncertainty in roty { - auto cov = generate_one_variable_covariance(1, 0.1); - auto cov_trans = rotate_translate({90., 0., 0.}, {20., 0., 0.}, cov); + 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); From f33a77234f5418216affe19240494a8efd8841ed Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Sun, 15 Dec 2019 22:16:01 -0800 Subject: [PATCH 4/7] Fix another compiler warning. Change function arguments to const ref --- gtsam/geometry/tests/testPose2.cpp | 6 +++--- gtsam/geometry/tests/testPose3.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index fec3fe426..0d53dc4e0 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -880,8 +880,8 @@ namespace transform_covariance3 { // Transform a covariance matrix with a rotation and a translation template static typename TPose::Jacobian RotateTranslate( - std::array r, - std::array t, + const std::array &r, + const std::array &t, const typename TPose::Jacobian &cov) { // Construct a pose object @@ -900,7 +900,7 @@ TEST(Pose2 , TransformCovariance3) { // rotate { - auto cov = GenerateFullCovariance({0.1, 0.3, 0.7}); + auto cov = GenerateFullCovariance({{0.1, 0.3, 0.7}}); auto cov_trans = RotateTranslate({{90.}}, {{}}, cov); // interchange x and y axes EXPECT_DOUBLES_EQUAL(cov_trans(1, 1), cov(0, 0), 1e-9); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 14abe3d8c..f9b388b9b 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -929,8 +929,8 @@ namespace transform_covariance6 { // Transform a covariance matrix with a rotation and a translation template static typename TPose::Jacobian RotateTranslate( - std::array r, - std::array t, + const std::array &r, + const std::array &t, const typename TPose::Jacobian &cov) { // Construct a pose object From 1f4297a0ec5a0250271916becc3561a36feced49 Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Sun, 15 Dec 2019 22:59:11 -0800 Subject: [PATCH 5/7] Move shared code into a header file --- gtsam/geometry/tests/testPose2.cpp | 59 +-------------- gtsam/geometry/tests/testPose3.cpp | 67 +---------------- gtsam/geometry/tests/testPoseAdjointMap.h | 87 +++++++++++++++++++++++ 3 files changed, 92 insertions(+), 121 deletions(-) create mode 100644 gtsam/geometry/tests/testPoseAdjointMap.h diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 0d53dc4e0..4343b5408 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -836,67 +836,12 @@ TEST(Pose2 , ChartDerivatives) { } //****************************************************************************** -namespace transform_covariance3 { - const double degree = M_PI / 180; +#include "testPoseAdjointMap.h" - // Create a covariance matrix for type T. Use sigma_values^2 on the diagonal - // and fill in non-diagonal entries with a correlation coefficient of 1. Note: - // a covariance matrix for T has the same dimensions as a Jacobian for T. - template - static typename T::Jacobian GenerateFullCovariance( - std::array sigma_values) - { - typename T::TangentVector sigmas(&sigma_values.front()); - return typename T::Jacobian{sigmas * sigmas.transpose()}; - } - - // Create a covariance matrix with one non-zero element on the diagonal. - template - static typename T::Jacobian GenerateOneVariableCovariance(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 - template - static typename T::Jacobian GenerateTwoVariableCovariance(int idx0, int idx1, double sigma0, double sigma1) - { - typename T::Jacobian cov = T::Jacobian::Zero(); - cov(idx0, idx0) = sigma0 * sigma0; - cov(idx1, idx1) = sigma1 * sigma1; - cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1; - return cov; - } - - // Overloaded function to create a Rot2 from one angle. - static Rot2 RotFromArray(const std::array &r) - { - return Rot2{r[0] * degree}; - } - - // Transform a covariance matrix with a rotation and a translation - template - static typename TPose::Jacobian RotateTranslate( - const std::array &r, - const std::array &t, - const typename TPose::Jacobian &cov) - { - // Construct a pose object - typename TPose::Rotation rot{RotFromArray(r)}; - TPose wTb{rot, typename TPose::Translation{&t.front()}}; - - // transform the covariance with the AdjointMap - auto adjointMap = wTb.AdjointMap(); - return adjointMap * cov * adjointMap.transpose(); - } -} TEST(Pose2 , TransformCovariance3) { // Use simple covariance matrices and transforms to create tests that can be // validated with simple computations. - using namespace transform_covariance3; + using namespace test_pose_adjoint_map; // rotate { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index f9b388b9b..97153238a 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -879,72 +879,11 @@ TEST(Pose3 , ChartDerivatives) { } //****************************************************************************** -namespace transform_covariance6 { - const double degree = M_PI / 180; +#include "testPoseAdjointMap.h" - // Create a covariance matrix for type T. Use sigma_values^2 on the diagonal - // and fill in non-diagonal entries with a correlation coefficient of 1. Note: - // a covariance matrix for T has the same dimensions as a Jacobian for T. - template - static typename T::Jacobian GenerateFullCovariance( - std::array sigma_values) - { - typename T::TangentVector sigmas(&sigma_values.front()); - return typename T::Jacobian{sigmas * sigmas.transpose()}; - } - - // Create a covariance matrix with one non-zero element on the diagonal. - template - static typename T::Jacobian GenerateOneVariableCovariance(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 - template - static typename T::Jacobian GenerateTwoVariableCovariance(int idx0, int idx1, double sigma0, double sigma1) - { - typename T::Jacobian cov = T::Jacobian::Zero(); - cov(idx0, idx0) = sigma0 * sigma0; - cov(idx1, idx1) = sigma1 * sigma1; - cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1; - return cov; - } - - // Overloaded function to create a Rot2 from one angle. - static Rot2 RotFromArray(const std::array &r) - { - return Rot2{r[0] * degree}; - } - - // Overloaded function to create a Rot3 from three angles. - static 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 - static typename TPose::Jacobian RotateTranslate( - const std::array &r, - const std::array &t, - const typename TPose::Jacobian &cov) - { - // Construct a pose object - typename TPose::Rotation rot{RotFromArray(r)}; - TPose wTb{rot, typename TPose::Translation{&t.front()}}; - - // transform the covariance with the AdjointMap - auto adjointMap = wTb.AdjointMap(); - return adjointMap * cov * adjointMap.transpose(); - } -} TEST(Pose3, TransformCovariance6MapTo2d) { // Create 3d scenarios that map to 2d configurations and compare with Pose2 results. - using namespace transform_covariance6; + using namespace test_pose_adjoint_map; std::array s{{0.1, 0.3, 0.7}}; std::array r{{31.}}; @@ -990,7 +929,7 @@ TEST(Pose3, TransformCovariance6MapTo2d) { TEST(Pose3, TransformCovariance6) { // Use simple covariance matrices and transforms to create tests that can be // validated with simple computations. - using namespace transform_covariance6; + using namespace test_pose_adjoint_map; // rotate 90 around z axis and then 90 around y axis { diff --git a/gtsam/geometry/tests/testPoseAdjointMap.h b/gtsam/geometry/tests/testPoseAdjointMap.h new file mode 100644 index 000000000..1263a807c --- /dev/null +++ b/gtsam/geometry/tests/testPoseAdjointMap.h @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testPoseAdjointMap.h + * @brief Support utilities for using AdjointMap for transforming Pose2 and Pose3 covariance matrices + * @author Peter Mulllen + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +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. + template + typename T::Jacobian GenerateFullCovariance( + std::array sigma_values) + { + typename T::TangentVector sigmas(&sigma_values.front()); + return typename T::Jacobian{sigmas * sigmas.transpose()}; + } + + // Create a covariance matrix with one non-zero element on the diagonal. + template + typename T::Jacobian GenerateOneVariableCovariance(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 + template + typename T::Jacobian GenerateTwoVariableCovariance(int idx0, int idx1, double sigma0, double sigma1) + { + typename T::Jacobian cov = T::Jacobian::Zero(); + cov(idx0, idx0) = sigma0 * sigma0; + cov(idx1, idx1) = sigma1 * sigma1; + 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 From 1236ab6c8e3f29869949213463aab10944c86567 Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Fri, 20 Dec 2019 23:15:56 -0800 Subject: [PATCH 6/7] Move TransformCovariance functor into lie.h --- gtsam/base/Lie.h | 10 +++ gtsam/geometry/tests/testPose2.cpp | 85 +++++++++--------- gtsam/geometry/tests/testPose3.cpp | 105 ++++++++++------------ gtsam/geometry/tests/testPoseAdjointMap.h | 52 +++-------- 4 files changed, 112 insertions(+), 140 deletions(-) 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 From 523d0a937f5870322d19be9b7c4f1a28db00bf9a Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Sat, 21 Dec 2019 12:22:32 -0800 Subject: [PATCH 7/7] Move TransformCovariance functor out of LieGroup class --- gtsam/base/Lie.h | 26 +++++++++++++++----------- gtsam/geometry/tests/testPose2.cpp | 12 ++++++------ gtsam/geometry/tests/testPose3.cpp | 16 ++++++++-------- 3 files changed, 29 insertions(+), 25 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index b6ec6a724..bbed26992 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -160,17 +160,6 @@ struct LieGroup { if (H2) *H2 = D_v_h; 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 @@ -346,6 +335,21 @@ T interpolate(const T& X, const T& Y, double t) { return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } +/** + * Functor for transforming covariance of T. + * T needs to satisfy the Lie group concept. + */ +template +class TransformCovariance +{ +private: + typename T::Jacobian adjointMap_; +public: + explicit TransformCovariance(const T &X) : adjointMap_{X.AdjointMap()} {} + typename T::Jacobian operator()(const typename T::Jacobian &covariance) + { return adjointMap_ * covariance * adjointMap_.transpose(); } +}; + } // namespace gtsam /** diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 81f097bd9..3d821502d 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -846,7 +846,7 @@ TEST(Pose2 , TransformCovariance3) { // rotate { auto cov = FullCovarianceFromSigmas({0.1, 0.3, 0.7}); - auto transformed = Pose2::TransformCovariance{{0., 0., 90 * degree}}(cov); + auto transformed = TransformCovariance{{0., 0., 90 * degree}}(cov); // interchange x and y axes EXPECT(assert_equal( Vector3{cov(1, 1), cov(0, 0), cov(2, 2)}, @@ -859,7 +859,7 @@ TEST(Pose2 , TransformCovariance3) { // translate along x with uncertainty in x { auto cov = SingleVariableCovarianceFromSigma(0, 0.1); - auto transformed = Pose2::TransformCovariance{{20., 0., 0.}}(cov); + auto transformed = TransformCovariance{{20., 0., 0.}}(cov); // No change EXPECT(assert_equal(cov, transformed)); } @@ -867,7 +867,7 @@ TEST(Pose2 , TransformCovariance3) { // translate along x with uncertainty in y { auto cov = SingleVariableCovarianceFromSigma(1, 0.1); - auto transformed = Pose2::TransformCovariance{{20., 0., 0.}}(cov); + auto transformed = TransformCovariance{{20., 0., 0.}}(cov); // No change EXPECT(assert_equal(cov, transformed)); } @@ -875,7 +875,7 @@ TEST(Pose2 , TransformCovariance3) { // translate along x with uncertainty in theta { auto cov = SingleVariableCovarianceFromSigma(2, 0.1); - auto transformed = Pose2::TransformCovariance{{20., 0., 0.}}(cov); + auto transformed = TransformCovariance{{20., 0., 0.}}(cov); EXPECT(assert_equal( Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1}, Vector3{transformed.diagonal()})); @@ -887,7 +887,7 @@ TEST(Pose2 , TransformCovariance3) { // rotate and translate along x with uncertainty in x { auto cov = SingleVariableCovarianceFromSigma(0, 0.1); - auto transformed = Pose2::TransformCovariance{{20., 0., 90 * degree}}(cov); + auto transformed = TransformCovariance{{20., 0., 90 * degree}}(cov); // interchange x and y axes EXPECT(assert_equal( Vector3{cov(1, 1), cov(0, 0), cov(2, 2)}, @@ -900,7 +900,7 @@ TEST(Pose2 , TransformCovariance3) { // rotate and translate along x with uncertainty in theta { auto cov = SingleVariableCovarianceFromSigma(2, 0.1); - auto transformed = Pose2::TransformCovariance{{20., 0., 90 * degree}}(cov); + auto transformed = TransformCovariance{{20., 0., 90 * degree}}(cov); EXPECT(assert_equal( Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1}, Vector3{transformed.diagonal()})); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index fc702d3b3..fedd47620 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -888,7 +888,7 @@ TEST(Pose3, TransformCovariance6MapTo2d) { 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 transformed2 = TransformCovariance{p2}(cov2); auto match_cov3_to_cov2 = [&](int spatial_axis0, int spatial_axis1, int r_axis, const Pose2::Jacobian &cov2, const Pose3::Jacobian &cov3) -> void @@ -904,21 +904,21 @@ TEST(Pose3, TransformCovariance6MapTo2d) { // rotate around x axis { 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); + auto transformed3 = 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 = 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); + auto transformed3 = 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 = 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); + auto transformed3 = TransformCovariance{{Rot3::RzRyRx(0., 0., p2.theta()), {p2.x(), p2.y(), 0.}}}(cov3); match_cov3_to_cov2(3, 4, 2, transformed2, transformed3); } } @@ -932,7 +932,7 @@ TEST(Pose3, TransformCovariance6) { // rotate 90 around z axis and then 90 around y axis { 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); + auto transformed = TransformCovariance{{Rot3::RzRyRx(0., 90 * degree, 90 * degree), {0., 0., 0.}}}(cov); // x from y, y from z, z from x EXPECT(assert_equal( (Vector6{} << cov(1, 1), cov(2, 2), cov(0, 0), cov(4, 4), cov(5, 5), cov(3, 3)).finished(), @@ -947,7 +947,7 @@ TEST(Pose3, TransformCovariance6) { // translate along the x axis with uncertainty in roty and rotz { auto cov = TwoVariableCovarianceFromSigmas(1, 2, 0.7, 0.3); - auto transformed = Pose3::TransformCovariance{{Rot3::RzRyRx(0., 0., 0.), {20., 0., 0.}}}(cov); + auto transformed = 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))); @@ -961,7 +961,7 @@ TEST(Pose3, TransformCovariance6) { // rotate around x axis and translate along the x axis with uncertainty in rotx { auto cov = SingleVariableCovarianceFromSigma(0, 0.1); - auto transformed = Pose3::TransformCovariance{{Rot3::RzRyRx(90 * degree, 0., 0.), {20., 0., 0.}}}(cov); + auto transformed = TransformCovariance{{Rot3::RzRyRx(90 * degree, 0., 0.), {20., 0., 0.}}}(cov); // No change EXPECT(assert_equal(cov, transformed)); } @@ -969,7 +969,7 @@ TEST(Pose3, TransformCovariance6) { // rotate around x axis and translate along the x axis with uncertainty in roty { auto cov = SingleVariableCovarianceFromSigma(1, 0.1); - auto transformed = Pose3::TransformCovariance{{Rot3::RzRyRx(90 * degree, 0., 0.), {20., 0., 0.}}}(cov); + auto transformed = 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(),