Merge pull request #192 from ptrmu/develop

Test using AdjointMap for transforming Pose2 and Pose3 covariance matrices from body to world frames

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<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
 }
 
+/**
+ * Functor for transforming covariance of T.
+ * T needs to satisfy the Lie group concept.
+ */
+template<class T>
+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
 
 /**

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<Pose2>({0.1, 0.3, 0.7});
+    auto transformed = TransformCovariance<Pose2>{{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<Pose2>(0, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
+    // No change
+    EXPECT(assert_equal(cov, transformed));
+  }
+
+  // translate along x with uncertainty in y
+  {
+    auto cov = SingleVariableCovarianceFromSigma<Pose2>(1, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
+    // No change
+    EXPECT(assert_equal(cov, transformed));
+  }
+
+  // translate along x with uncertainty in theta
+  {
+    auto cov = SingleVariableCovarianceFromSigma<Pose2>(2, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{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<Pose2>(0, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{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<Pose2>(2, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{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;

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<Pose2>(s2);
+  auto transformed2 = TransformCovariance<Pose2>{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<Pose3>((Vector6{} << s2(2), 0., 0., 0., s2(0), s2(1)).finished());
+    auto transformed3 = TransformCovariance<Pose3>{{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<Pose3>((Vector6{} << 0., s2(2), 0., s2(1), 0., s2(0)).finished());
+    auto transformed3 = TransformCovariance<Pose3>{{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<Pose3>((Vector6{} << 0., 0., s2(2), s2(0), s2(1), 0.).finished());
+    auto transformed3 = TransformCovariance<Pose3>{{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<Pose3>((Vector6{} << 0.1, 0.2, 0.3, 0.5, 0.7, 1.1).finished());
+    auto transformed = TransformCovariance<Pose3>{{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<Pose3>(1, 2, 0.7, 0.3);
+    auto transformed = TransformCovariance<Pose3>{{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<Pose3>(0, 0.1);
+    auto transformed = TransformCovariance<Pose3>{{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<Pose3>(1, 0.1);
+    auto transformed = TransformCovariance<Pose3>{{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)));

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 <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+
+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<class T>
+  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<class T>
+  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<class T>
+  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;
+  }
+}