Add Lie group capabilities to NavState

gtsam/geometry/Pose3.h

@@ -135,10 +135,10 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
   /// @name Lie Group
   /// @{
 
-  /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
+  /// Exponential map at identity - create a pose from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
   static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {});
 
-  /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
+  /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this pose
   static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {});
 
   /**

gtsam/geometry/Rot3.cpp

@@ -121,7 +121,7 @@ Unit3 Rot3::unrotate(const Unit3& p,
     OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
   Matrix32 Dp;
   Unit3 q = Unit3(unrotate(p.point3(Dp)));
-  if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
+  if (Hp) *Hp = q.basis().transpose() * matrix().transpose() * Dp;
   if (HR) *HR = q.basis().transpose() * q.skew();
   return q;
 }

gtsam/navigation/NavState.cpp

@@ -188,6 +188,31 @@ Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state,
   return Ad * xi_b;
 }
 
+//------------------------------------------------------------------------------
+/// The dual version of Adjoint
+Vector9 NavState::AdjointTranspose(const Vector9& x, OptionalJacobian<9, 9> H_state,
+                                OptionalJacobian<9, 9> H_x) const {
+  const Matrix9 Ad = AdjointMap();
+  const Vector9 AdTx = Ad.transpose() * x;
+
+  // Jacobians
+  if (H_state) {
+    //TODO(Varun)
+    // const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
+    //            v_T_hat = skewSymmetric(AdTx.segment<3>(3)),
+    //            a_T_hat = skewSymmetric(AdTx.tail<3>());
+    //   *H_state << w_T_hat, v_T_hat,  //
+    //       /*  */ v_T_hat, Z_3x3;
+    throw std::runtime_error(
+        "AdjointTranpose H_state Jacobian not implemented.");
+  }
+  if (H_x) {
+    *H_x = Ad.transpose();
+  }
+
+  return AdTx;
+}
+
 //------------------------------------------------------------------------------
 Matrix9 NavState::adjointMap(const Vector9& xi) {
   Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
@@ -219,6 +244,25 @@ Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y,
   return ad_xi * y;
 }
 
+//------------------------------------------------------------------------------
+Vector9 NavState::adjointTranspose(const Vector9& xi, const Vector9& y,
+                                   OptionalJacobian<9, 9> Hxi,
+                                   OptionalJacobian<9, 9> H_y) {
+  if (Hxi) {
+    Hxi->setZero();
+    for (int i = 0; i < 9; ++i) {
+      Vector9 dxi;
+      dxi.setZero();
+      dxi(i) = 1.0;
+      Matrix9 GTi = adjointMap(dxi).transpose();
+      Hxi->col(i) = GTi * y;
+    }
+  }
+  const Matrix9& adT_xi = adjointMap(xi).transpose();
+  if (H_y) *H_y = adT_xi;
+  return adT_xi * y;
+}
+
 //------------------------------------------------------------------------------
 Matrix9 NavState::ExpmapDerivative(const Vector9& xi) {
   Matrix9 J;
@@ -291,6 +335,30 @@ NavState NavState::retract(const Vector9& xi, //
 //------------------------------------------------------------------------------
 Vector9 NavState::localCoordinates(const NavState& g, //
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
+  // return LieGroup<NavState, 9>::localCoordinates(g, H1, H2);
+
+  //TODO(Varun) Fix so that test on L680 passes
+
+  // Matrix3 D_dR_R, D_dt_R, D_dv_R;
+  // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
+  // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
+  // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
+
+  // Vector9 xi;
+  // Matrix3 D_xi_R;
+  // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
+  // if (H1) {
+  //   *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
+  //   D_dt_R, -I_3x3, Z_3x3, //
+  //   D_dv_R, Z_3x3, -I_3x3;
+  // }
+  // if (H2) {
+  //   *H2 << D_xi_R, Z_3x3, Z_3x3, //
+  //   Z_3x3, dR.matrix(), Z_3x3, //
+  //   Z_3x3, Z_3x3, dR.matrix();
+  // }
+  // return xi;
+
   Matrix3 D_dR_R, D_dt_R, D_dv_R;
   const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
   const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);

gtsam/navigation/NavState.h

@@ -217,6 +217,11 @@ class GTSAM_EXPORT NavState : public LieGroup<NavState, 9> {
                   OptionalJacobian<9, 9> H_this = {},
                   OptionalJacobian<9, 9> H_xib = {}) const;
 
+  /// The dual version of Adjoint
+  Vector9 AdjointTranspose(const Vector9& x,
+                           OptionalJacobian<9, 9> H_this = {},
+                           OptionalJacobian<9, 9> H_x = {}) const;
+
   /**
    * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
    * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^]
@@ -230,6 +235,13 @@ class GTSAM_EXPORT NavState : public LieGroup<NavState, 9> {
                          OptionalJacobian<9, 9> Hxi = {},
                          OptionalJacobian<9, 9> H_y = {});
 
+  /**
+   * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
+   */
+  static Vector9 adjointTranspose(const Vector9& xi, const Vector9& y,
+                                  OptionalJacobian<9, 9> Hxi = {},
+                                  OptionalJacobian<9, 9> H_y = {});
+
   /// Derivative of Expmap
   static Matrix9 ExpmapDerivative(const Vector9& xi);
 

gtsam/navigation/tests/testNavState.cpp

@@ -225,7 +225,7 @@ TEST(NavState, Compose) {
 }
 
 /* ************************************************************************* */
-// Check compose and its push-forward, another case
+// Check compose and its pushforward, another case
 TEST(NavState, Compose2) {
   const NavState& T1 = T;
   Matrix actual = (T1 * T2).matrix();
@@ -322,6 +322,49 @@ TEST(NavState, interpolate) {
   EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0)));
 }
 
+/* ************************************************************************* */
+TEST(NavState, Lie) {
+  NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0});
+  NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0});
+  NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0},
+                       {3.0, -1.0, 1.0});
+
+  // TODO(Varun)
+  // logmap
+  // Matrix9 H1, H2;
+  // auto logmap_b = NavState::Create(Rot3::Identity(),
+  //                                         Vector3::Zero(), Vector3::Zero())
+  //                     .localCoordinates(nav_state_b, H1, H2);
+
+  // Matrix6 J1, J2;
+  // auto logmap_pose_b = Pose3::Create(Rot3(), Vector3::Zero())
+  //                          .localCoordinates(nav_state_b.pose(), J1, J2);
+
+  // //  Check retraction
+  // auto retraction_b = NavState().retract(logmap_b);
+  // CHECK(assert_equal(retraction_b, nav_state_b));
+
+  // // Test if the sum of the logmap is the same as the logmap of the product
+  // auto logmap_c = NavState::Create(Rot3::Identity(),
+  //                                         Vector3::Zero(), Vector3::Zero())
+  //                     .localCoordinates(nav_state_c);
+
+  // auto logmap_bc = NavState::Create(
+  //                       gtsam::Rot3::Identity(), Eigen::Vector3d::Zero(),
+  //                       Eigen::Vector3d::Zero(), {}, {}, {})
+  //                       .localCoordinates(nav_state_b * nav_state_c);
+  // Vector9 logmap_bc2 = NavState::Logmap(nav_state_b * nav_state_c);
+
+  // Vector9 logmap_bc_sum = logmap_b + logmap_c;
+  // std::cout << "logmap_bc = " << logmap_bc.transpose() << std::endl;
+  // std::cout << "logmap_bc2 = " << logmap_bc2.transpose() << std::endl;
+
+  // // std::cout << "logmap_bc + logmap_c = " << logmap_bc_sum.transpose() << std::endl;
+  // // std::cout << "logmap_b + logmap_c = " << (NavState::Logmap(nav_state_b) + NavState::Logmap(nav_state_c)).transpose() << std::endl;
+  // // std::cout << "logmap_bc = " << logmap_bc.transpose() << std::endl;
+  // // CHECK(assert_equal(logmap_bc_sum, logmap_bc));
+}
+
 /* ************************************************************************* */
 static const double dt = 2.0;
 std::function<Vector9(const NavState&, const bool&)> coriolis =
@@ -504,6 +547,16 @@ Point3 expectedP(0.29552, 0.0446635, 1);
 NavState expected(expectedR, expectedV, expectedP);
 }  // namespace screwNavState
 
+/* ************************************************************************* */
+// Checks correct exponential map (Expmap) with brute force matrix exponential
+TEST(NavState, Expmap_c_full) {
+  //TODO(Varun)
+  // EXPECT(assert_equal(screwNavState::expected,
+  //                     expm<NavState>(screwNavState::xi), 1e-6));
+  // EXPECT(assert_equal(screwNavState::expected,
+  //                     NavState::Expmap(screwNavState::xi), 1e-6));
+}
+
 /* ************************************************************************* */
 // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
 TEST(NavState, Adjoint_full) {
@@ -520,19 +573,70 @@ TEST(NavState, Adjoint_full) {
   EXPECT(assert_equal(expected3, NavState::Expmap(xiPrime3), 1e-6));
 }
 
+/* ************************************************************************* */
+// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
+TEST(NavState, Adjoint_hat) {
+  //TODO(Varun)
+  // auto hat = [](const Vector& xi) { return ::wedge<NavState>(xi); };
+  // Matrix5 expected = T.matrix() * hat(screwNavState::xi) * T.matrix().inverse();
+  // Matrix5 xiprime = hat(T.Adjoint(screwNavState::xi));
+
+  // EXPECT(assert_equal(expected, xiprime, 1e-6));
+
+  // Matrix5 expected2 =
+  //     T2.matrix() * hat(screwNavState::xi) * T2.matrix().inverse();
+  // Matrix5 xiprime2 = hat(T2.Adjoint(screwNavState::xi));
+  // EXPECT(assert_equal(expected2, xiprime2, 1e-6));
+
+  // Matrix5 expected3 =
+  //     T3.matrix() * hat(screwNavState::xi) * T3.matrix().inverse();
+
+  // Matrix5 xiprime3 = hat(T3.Adjoint(screwNavState::xi));
+  // EXPECT(assert_equal(expected3, xiprime3, 1e-6));
+}
+
 /* ************************************************************************* */
 TEST(NavState, Adjoint_compose_full) {
   // To debug derivatives of compose, assert that
   // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
   const NavState& T1 = T;
-  Vector x =
-      (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished();
+  Vector9 x;
+  x << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8;
   NavState expected = T1 * NavState::Expmap(x) * T2;
   Vector y = T2.inverse().Adjoint(x);
   NavState actual = T1 * T2 * NavState::Expmap(y);
   EXPECT(assert_equal(expected, actual, 1e-6));
 }
 
+/* ************************************************************************* */
+TEST(NavState, expmaps_galore_full) {
+  Vector xi;
+  //TODO(Varun)
+  // NavState actual;
+  // xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished();
+  // actual = NavState::Expmap(xi);
+  // EXPECT(assert_equal(expm<NavState>(xi), actual, 1e-6));
+  // EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-6));
+
+  // xi = (Vector(9) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6, -0.7, -0.8, -0.9)
+  //          .finished();
+  // for (double theta = 1.0; 0.3 * theta <= M_PI; theta *= 2) {
+  //   Vector txi = xi * theta;
+  //   actual = NavState::Expmap(txi);
+  //   EXPECT(assert_equal(expm<NavState>(txi, 30), actual, 1e-6));
+  //   Vector log = NavState::Logmap(actual);
+  //   EXPECT(assert_equal(actual, NavState::Expmap(log), 1e-6));
+  //   EXPECT(assert_equal(txi, log, 1e-6));  // not true once wraps
+  // }
+
+  // // Works with large v as well, but expm needs 10 iterations!
+  // xi =
+  //     (Vector(9) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0, 12, 14, 45).finished();
+  // actual = NavState::Expmap(xi);
+  // EXPECT(assert_equal(expm<NavState>(xi, 10), actual, 1e-5));
+  // EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-9));
+}
+
 /* ************************************************************************* */
 TEST(NavState, Retract_LocalCoordinates) {
   Vector9 d;
@@ -586,8 +690,8 @@ TEST(NavState, manifold_expmap) {
 TEST(NavState, subgroups) {
   // Frank - Below only works for correct "Agrawal06iros style expmap
   // lines in canonical coordinates correspond to Abelian subgroups in SE(3)
-  Vector d =
-      (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished();
+  Vector9 d;
+  d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9;
   // exp(-d)=inverse(exp(d))
   EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse()));
   // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)