Separate adjointMap, which returns a matrix, and adjoint+adjointTranspose actions on a vector with optional derivatives. Also, change dExpInv_TLN to dExpInv_exp with higher order of approximation

gtsam/geometry/Pose3.cpp

@@ -55,22 +55,50 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Matrix6 Pose3::adjoint(const Vector& xi) {
+  Matrix6 Pose3::adjointMap(const Vector& xi) {
     Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
     Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
     Matrix6 adj;
     adj << w_hat, Z3, v_hat, w_hat;
+
     return adj;
   }
 
   /* ************************************************************************* */
-  Matrix6 Pose3::dExpInv_TLN(const Vector& xi) {
+  Vector Pose3::adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
+    if (H) {
+      *H = zeros(6,6);
+      for (int i = 0; i<6; ++i) {
+        Vector dxi = zero(6); dxi(i) = 1.0;
+        Matrix Gi = adjointMap(dxi);
+        (*H).col(i) = Gi*y;
+      }
+    }
+    return adjointMap(xi)*y;
+  }
+
+  /* ************************************************************************* */
+  Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
+    if (H) {
+      *H = zeros(6,6);
+      for (int i = 0; i<6; ++i) {
+        Vector dxi = zero(6); dxi(i) = 1.0;
+        Matrix GTi = adjointMap(dxi).transpose();
+        (*H).col(i) = GTi*y;
+      }
+    }
+    Matrix adjT = adjointMap(xi).transpose();
+    return adjointMap(xi).transpose() * y;
+  }
+
+  /* ************************************************************************* */
+  Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
     // Bernoulli numbers, from Wikipedia
     static const Vector B = Vector_(9, 1.0, -1.0/2.0, 1./6., 0.0, -1.0/30.0, 0.0, 1.0/42.0, 0.0, -1.0/30);
     static const int N = 5; // order of approximation
     Matrix res = I6;
     Matrix6 ad_i = I6;
-    Matrix6 ad_xi = adjoint(xi);
+    Matrix6 ad_xi = adjointMap(xi);
     double fac = 1.0;
     for (int i = 1 ; i<N; ++i) {
       ad_i = ad_xi * ad_i;

gtsam/geometry/Pose3.h

@@ -19,7 +19,7 @@
 #pragma once
 
 #ifndef POSE3_DEFAULT_COORDINATES_MODE
-#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
+#define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP
 #endif
 
 #include <gtsam/base/DerivedValue.h>
@@ -171,17 +171,29 @@ namespace gtsam {
      * and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
      *
      */
-    static Matrix6 adjoint(const Vector& xi);
+    static Matrix6 adjointMap(const Vector& xi);
+
+    /**
+     * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
+     */
+    static Vector adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
+
+    /**
+     * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
+     */
+    static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
 
     /**
      * Compute the inverse right-trivialized tangent (derivative) map of the exponential map,
-     * using the trapezoidal Lie-Newmark (TLN) scheme
+     * as detailed in [Kobilarov09siggraph] eq. (15)
-     * as detailed in [Kobilarov09siggraph] eq. (15) and C_TLN.
      * The full formula is documented in [Celledoni99cmame]
      *    Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and
      *    time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421<EFBFBD> 438, 2003.
+     * and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2
+     *    Ernst Hairer, et al., Geometric Numerical Integration,
+     *      Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006.
      */
-    static Matrix6 dExpInv_TLN(const Vector&  xi);
+    static Matrix6 dExpInv_exp(const Vector&  xi);
 
     /**
      * wedge for Pose3:

gtsam/geometry/tests/testPose3.cpp

@@ -627,8 +627,8 @@ TEST( Pose3, unicycle )
 }
 
 /* ************************************************************************* */
-TEST( Pose3, adjoint) {
+TEST( Pose3, adjointMap) {
-  Matrix res = Pose3::adjoint(screw::xi);
+  Matrix res = Pose3::adjointMap(screw::xi);
   Matrix wh = skewSymmetric(screw::xi(0), screw::xi(1), screw::xi(2));
   Matrix vh = skewSymmetric(screw::xi(3), screw::xi(4), screw::xi(5));
   Matrix Z3 = zeros(3,3);
@@ -674,12 +674,16 @@ TEST(Pose3, align_2) {
 /* ************************************************************************* */
 /// exp(xi) exp(y) = exp(xi + x)
 /// Hence, y = log (exp(-xi)*exp(xi+x))
+
+Vector xi = Vector_(6, 0.1, 0.2, 0.3, 1.0, 2.0, 3.0);
+
 Vector testDerivExpmapInv(const LieVector& dxi) {
-  return Pose3::Logmap(Pose3::Expmap(-screw::xi)*Pose3::Expmap(screw::xi+dxi));
+  Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi));
+  return y;
 }
 
 TEST( Pose3, dExpInv_TLN) {
-  Matrix res = Pose3::dExpInv_TLN(screw::xi);
+  Matrix res = Pose3::dExpInv_exp(xi);
 
   Matrix numericalDerivExpmapInv = numericalDerivative11(
       boost::function<Vector(const LieVector&)>(
@@ -688,7 +692,53 @@ TEST( Pose3, dExpInv_TLN) {
       LieVector(Vector::Zero(6)), 1e-5
       );
 
-  EXPECT(assert_equal(numericalDerivExpmapInv,res,1e-1));
+  EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1));
+}
+
+/* ************************************************************************* */
+Vector testDerivAdjoint(const LieVector& xi, const LieVector& v) {
+  return Pose3::adjointMap(xi)*v;
+}
+
+TEST( Pose3, adjoint) {
+  Vector expected = testDerivAdjoint(screw::xi, screw::xi);
+
+  Matrix actualH;
+  Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH);
+
+  Matrix numericalH = numericalDerivative21(
+      boost::function<Vector(const LieVector&, const LieVector&)>(
+          boost::bind(testDerivAdjoint,  _1, _2)
+          ),
+      LieVector(screw::xi), LieVector(screw::xi), 1e-5
+      );
+
+  EXPECT(assert_equal(expected,actual,1e-5));
+  EXPECT(assert_equal(numericalH,actualH,1e-5));
+}
+
+/* ************************************************************************* */
+Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) {
+  return Pose3::adjointMap(xi).transpose()*v;
+}
+
+TEST( Pose3, adjointTranspose) {
+  Vector xi = Vector_(6, 0.01, 0.02, 0.03, 1.0, 2.0, 3.0);
+  Vector v = Vector_(6, 0.04, 0.05, 0.06, 4.0, 5.0, 6.0);
+  Vector expected = testDerivAdjointTranspose(xi, v);
+
+  Matrix actualH;
+  Vector actual = Pose3::adjointTranspose(xi, v, actualH);
+
+  Matrix numericalH = numericalDerivative21(
+      boost::function<Vector(const LieVector&, const LieVector&)>(
+          boost::bind(testDerivAdjointTranspose,  _1, _2)
+          ),
+      LieVector(xi), LieVector(v), 1e-5
+      );
+
+  EXPECT(assert_equal(expected,actual,1e-15));
+  EXPECT(assert_equal(numericalH,actualH,1e-5));
 }
 
 /* ************************************************************************* */