Pose3::Adjoint(xi) Jacobians

gtsam/geometry/Pose3.cpp

@@ -63,6 +63,87 @@ 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, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr);
+  const Vector3 Rv =
+      R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr);
+  // Since we use the Point3 version of cross, the jacobian of pRw wrpt t
+  // (pRw_H_t) needs special treatment as detailed below.
+  const Vector3 pRw =
+      cross(t_, Rw, boost::none, (H_this || H_xib) ? &pRw_H_Rw : nullptr);
+  result.tail<3>() = pRw + Rv;
+
+  // Jacobians
+  if (H_this) {
+    // By applying the chain rule to the matrix-matrix product of [t]R, we can
+    // compute a simplified derivative which is the same as Rw_H_R. Details:
+    // https://github.com/borglab/gtsam/pull/885#discussion_r726591370
+    pRw_H_t = Rw_H_R;
+
+    *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;
+}
+
+/* ************************************************************************* */
+/// The dual version of Adjoint
+Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this,
+                                OptionalJacobian<6, 6> H_x) const {
+  //  Ad^T   *   xi   =   [ R^T  R^T.[-t]  .  [w
+  //                         0     R^T ]       v]
+  // Declarations and aliases
+  Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, tv_H_t, tv_H_v, Rtv_H_R, Rtv_H_tv;
+  Vector6 result;
+  const Vector3 &w = x.head<3>(), &v = x.tail<3>();
+  auto Rv = result.tail<3>();
+
+  // Calculations
+  const Vector3 Rw =
+      R_.unrotate(w, H_this ? &Rw_H_R : nullptr, H_x ? &Rw_H_w : nullptr);
+  Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr, H_x ? &Rv_H_v : nullptr);
+  const Vector3 tv =
+      cross(t_, v, H_this ? &tv_H_t : nullptr, H_x ? &tv_H_v : nullptr);
+  const Vector3 Rtv = R_.unrotate(tv, H_this ? &Rtv_H_R : nullptr,
+                                  (H_this || H_x) ? &Rtv_H_tv : nullptr);
+  result.head<3>() = Rw - Rtv;
+
+  // Jacobians
+  if (H_this) {
+    *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rv_H_R,  // -Rtv_H_tv * tv_H_t
+               /*        */ Rv_H_R, /*     */ Z_3x3)
+                  .finished();
+  }
+  if (H_x) {
+    *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v,  //
+            /*        */ Z_3x3, Rv_H_v)
+               .finished();
+  }
+
+  // Return
+  return result;
+}
+
 /* ************************************************************************* */
 Matrix6 Pose3::adjointMap(const Vector6& xi) {
   Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));

gtsam/geometry/Pose3.h

@@ -148,12 +148,19 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
   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;
+
+  /// The dual version of Adjoint
+  Vector6 AdjointTranspose(const Vector6& x,
+                           OptionalJacobian<6, 6> H_this = boost::none,
+                           OptionalJacobian<6, 6> H_x = boost::none) const;
 
   /**
    * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11

gtsam/geometry/tests/testPose3.cpp

@@ -145,6 +145,81 @@ TEST(Pose3, Adjoint_full)
   EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
 }
 
+/* ************************************************************************* */
+// Check Adjoint numerical derivatives
+TEST(Pose3, Adjoint_jacobians)
+{
+  Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
+
+  // Check evaluation sanity check
+  EQUALITY(static_cast<gtsam::Vector>(T.AdjointMap() * xi), T.Adjoint(xi));
+  EQUALITY(static_cast<gtsam::Vector>(T2.AdjointMap() * xi), T2.Adjoint(xi));
+  EQUALITY(static_cast<gtsam::Vector>(T3.AdjointMap() * xi), T3.Adjoint(xi));
+
+  // Check 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); };
+
+  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));
+}
+
+/* ************************************************************************* */
+// Check AdjointTranspose and jacobians
+TEST(Pose3, AdjointTranspose)
+{
+  Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
+
+  // Check evaluation
+  EQUALITY(static_cast<Vector>(T.AdjointMap().transpose() * xi),
+           T.AdjointTranspose(xi));
+  EQUALITY(static_cast<Vector>(T2.AdjointMap().transpose() * xi),
+           T2.AdjointTranspose(xi));
+  EQUALITY(static_cast<Vector>(T3.AdjointMap().transpose() * xi),
+           T3.AdjointTranspose(xi));
+
+  // Check jacobians
+  Matrix6 actualH1, actualH2, expectedH1, expectedH2;
+  std::function<Vector6(const Pose3&, const Vector6&)> AdjointTranspose_proxy =
+      [&](const Pose3& T, const Vector6& xi) {
+        return T.AdjointTranspose(xi);
+      };
+
+  T.AdjointTranspose(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi);
+  expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi);
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
+  EXPECT(assert_equal(expectedH2, actualH2));
+
+  T2.AdjointTranspose(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi);
+  expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi);
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
+  EXPECT(assert_equal(expectedH2, actualH2));
+
+  T3.AdjointTranspose(xi, actualH1, actualH2);
+  expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi);
+  expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi);
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
+  EXPECT(assert_equal(expectedH2, actualH2));
+}
+
 /* ************************************************************************* */
 // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
 TEST(Pose3, Adjoint_hat)