diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index bb49a84d6..bbed26992 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -160,7 +160,6 @@ struct LieGroup { if (H2) *H2 = D_v_h; return v; } - }; /// tag to assert a type is a Lie group @@ -336,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 b8b3fc52c..3d821502d 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -835,6 +835,81 @@ TEST(Pose2 , ChartDerivatives) { CHECK_CHART_DERIVATIVES(T2,T1); } +//****************************************************************************** +#include "testPoseAdjointMap.h" + +TEST(Pose2 , TransformCovariance3) { + // Use simple covariance matrices and transforms to create tests that can be + // validated with simple computations. + using namespace test_pose_adjoint_map; + + // rotate + { + auto cov = FullCovarianceFromSigmas({0.1, 0.3, 0.7}); + 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)}, + 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 = SingleVariableCovarianceFromSigma(0, 0.1); + auto transformed = TransformCovariance{{20., 0., 0.}}(cov); + // No change + EXPECT(assert_equal(cov, transformed)); + } + + // translate along x with uncertainty in y + { + auto cov = SingleVariableCovarianceFromSigma(1, 0.1); + auto transformed = TransformCovariance{{20., 0., 0.}}(cov); + // No change + EXPECT(assert_equal(cov, transformed)); + } + + // translate along x with uncertainty in theta + { + auto cov = SingleVariableCovarianceFromSigma(2, 0.1); + 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()})); + 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 = SingleVariableCovarianceFromSigma(0, 0.1); + 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)}, + 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 = SingleVariableCovarianceFromSigma(2, 0.1); + 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()})); + EXPECT(assert_equal( + Vector3{0., 0., -0.1 * 0.1 * 20.}, + Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)})); + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 0b2f59773..fedd47620 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -878,6 +878,105 @@ TEST(Pose3 , ChartDerivatives) { } } +//****************************************************************************** +#include "testPoseAdjointMap.h" + +TEST(Pose3, TransformCovariance6MapTo2d) { + // Create 3d scenarios that map to 2d configurations and compare with Pose2 results. + using namespace test_pose_adjoint_map; + + Vector3 s2{0.1, 0.3, 0.7}; + Pose2 p2{1.1, 1.5, 31. * degree}; + auto cov2 = FullCovarianceFromSigmas(s2); + 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 + { + 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 = FullCovarianceFromSigmas((Vector6{} << s2(2), 0., 0., 0., s2(0), s2(1)).finished()); + 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 = 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 = TransformCovariance{{Rot3::RzRyRx(0., 0., p2.theta()), {p2.x(), p2.y(), 0.}}}(cov3); + match_cov3_to_cov2(3, 4, 2, transformed2, transformed3); + } +} + +/* ************************************************************************* */ +TEST(Pose3, TransformCovariance6) { + // Use simple covariance matrices and transforms to create tests that can be + // validated with simple computations. + using namespace test_pose_adjoint_map; + + // 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 = 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(), + Vector6{transformed.diagonal()})); + // Both the x and z axes are pointing in the negative direction. + 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 = TwoVariableCovarianceFromSigmas(1, 2, 0.7, 0.3); + 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))); + 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 = SingleVariableCovarianceFromSigma(0, 0.1); + auto transformed = 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 = SingleVariableCovarianceFromSigma(1, 0.1); + 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(), + Vector6{transformed.diagonal()})); + } +} + /* ************************************************************************* */ TEST(Pose3, interpolate) { EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0))); diff --git a/gtsam/geometry/tests/testPoseAdjointMap.h b/gtsam/geometry/tests/testPoseAdjointMap.h new file mode 100644 index 000000000..1d006c3d9 --- /dev/null +++ b/gtsam/geometry/tests/testPoseAdjointMap.h @@ -0,0 +1,59 @@ +/* ---------------------------------------------------------------------------- + + * 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; + + // 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 FullCovarianceFromSigmas( + const typename T::TangentVector &sigmas) + { + return sigmas * sigmas.transpose(); + } + + // Return a covariance matrix with one non-zero element on the diagonal. + template + typename T::Jacobian SingleVariableCovarianceFromSigma(int idx, double sigma) + { + typename T::Jacobian cov = T::Jacobian::Zero(); + cov(idx, idx) = sigma * sigma; + return cov; + } + + // 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 TwoVariableCovarianceFromSigmas(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; + } +} \ No newline at end of file