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);