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