From fe59f3db19a0b56a18d560e482c48890bfbd8258 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Mon, 4 Oct 2021 03:56:25 -0400
Subject: [PATCH] AdjointMap jacobians for Pose3

---
 gtsam/geometry/Pose3.cpp           | 36 ++++++++++++++++++++++++++++++
 gtsam/geometry/Pose3.h             | 10 +++++----
 gtsam/geometry/tests/testPose3.cpp | 30 +++++++++++++++++++++++++
 3 files changed, 72 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index c183e32ed..93d6248e6 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -63,6 +63,42 @@ Matrix6 Pose3::AdjointMap() const {
   return adj;
 }
 
+/* ************************************************************************* */
+// Calculate AdjointMap applied to xi_b, with Jacobians
+Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this,
+                       OptionalJacobian<6, 6> H_xib) const {
+  //  Ad   *   xi   =   [   R   0    *   [w
+  //                      [t]R  R ]       v]
+
+  // Declarations and aliases
+  Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, pRw_H_t, pRw_H_Rw;
+  Vector6 result;
+  auto Rw = result.head<3>();
+  const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>();
+
+  // Calculations
+  Rw = R_.rotate(w, Rw_H_R, Rw_H_w);
+  const Vector3 Rv = R_.rotate(v, Rv_H_R, Rv_H_v);
+  const Vector3 pRw = cross(t_, Rw, pRw_H_t, pRw_H_Rw);
+  result.tail<3>() = pRw + Rv;
+  pRw_H_t = Rw_H_R;  // This is needed to pass the unit tests for some reason
+
+  // Jacobians
+  if (H_this) {
+    *H_this = (Matrix6() << Rw_H_R, /*               */ Z_3x3,  //
+               /*        */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t)
+                  .finished();
+  }
+  if (H_xib) {
+    *H_xib = (Matrix6() << Rw_H_w, /*      */ Z_3x3,  //
+              /*        */ pRw_H_Rw * Rw_H_w, Rv_H_v)
+                 .finished();
+  }
+
+  // Return
+  return result;
+}
+
 /* ************************************************************************* */
 Matrix6 Pose3::adjointMap(const Vector6& xi) {
   Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index d76e1b41a..9a8a7e939 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -148,12 +148,14 @@ public:
   Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect
 
   /**
-   * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame
+   * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a
+   * body-fixed velocity, transforming it to the spatial frame
    * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
+   * Note that H_xib = AdjointMap()
    */
-  Vector6 Adjoint(const Vector6& xi_b) const {
-    return AdjointMap() * xi_b;
-  } /// FIXME Not tested - marked as incorrect
+  Vector6 Adjoint(const Vector6& xi_b,
+                  OptionalJacobian<6, 6> H_this = boost::none,
+                  OptionalJacobian<6, 6> H_xib = boost::none) const;
 
   /**
    * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 7c1fa81e6..9fde4c2c1 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -145,6 +145,36 @@ TEST(Pose3, Adjoint_full)
   EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
 }
 
+/* ************************************************************************* */
+// Check Adjoint numerical derivatives
+TEST(Pose3, Adjoint_jacobians)
+{
+  Matrix6 actualH1, actualH2, expectedH1, expectedH2;
+  std::function<Vector6(const Pose3&, const Vector6&)> Adjoint_proxy =
+      [](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); };
+
+  gtsam::Vector6 xi =
+      (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
+
+  T.Adjoint(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi);
+  expectedH2 = numericalDerivative22(Adjoint_proxy, T, xi);
+  EXPECT(assert_equal(expectedH1, actualH1));
+  EXPECT(assert_equal(expectedH2, actualH2));
+
+  T2.Adjoint(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(Adjoint_proxy, T2, xi);
+  expectedH2 = numericalDerivative22(Adjoint_proxy, T2, xi);
+  EXPECT(assert_equal(expectedH1, actualH1));
+  EXPECT(assert_equal(expectedH2, actualH2));
+
+  T3.Adjoint(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(Adjoint_proxy, T3, xi);
+  expectedH2 = numericalDerivative22(Adjoint_proxy, T3, xi);
+  EXPECT(assert_equal(expectedH1, actualH1));
+  EXPECT(assert_equal(expectedH2, actualH2));
+}
+
 /* ************************************************************************* */
 // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
 TEST(Pose3, Adjoint_hat)