Add AdjointMap into traits

gtsam/base/Lie.h

@@ -169,32 +169,34 @@ namespace internal {
 /// To use this for your gtsam type, define:
 /// template<> struct traits<Class> : public internal::LieGroupTraits<Class> {};
 /// Assumes existence of: identity, dimension, localCoordinates, retract,
-/// and additionally Logmap, Expmap, compose, between, and inverse
+/// and additionally Logmap, Expmap, AdjointMap, compose, between, and inverse
 template<class Class>
-struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
+struct LieGroupTraits : public GetDimensionImpl<Class, Class::dimension> {
   using structure_category = lie_group_tag;
 
   /// @name Group
   /// @{
   using group_flavor = multiplicative_group_tag;
-  static Class Identity() { return Class::Identity();}
+  static Class Identity() { return Class::Identity(); }
   /// @}
 
   /// @name Manifold
   /// @{
   using ManifoldType = Class;
+  // Note: Class::dimension can be an int or Eigen::Dynamic.
+  // GetDimensionImpl handles resolving this to a static value or providing GetDimension(obj).
   inline constexpr static auto dimension = Class::dimension;
   using TangentVector = Eigen::Matrix<double, dimension, 1>;
   using ChartJacobian = OptionalJacobian<dimension, dimension>;
 
   static TangentVector Local(const Class& origin, const Class& other,
-      ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+    ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
-    return origin.localCoordinates(other, Horigin, Hother);
+    return origin.localCoordinates(other, H1, H2);
   }
 
   static Class Retract(const Class& origin, const TangentVector& v,
-      ChartJacobian Horigin = {}, ChartJacobian Hv = {}) {
+    ChartJacobian H = {}, ChartJacobian Hv = {}) {
-    return origin.retract(v, Horigin, Hv);
+    return origin.retract(v, H, Hv);
   }
   /// @}
 
@@ -222,6 +224,13 @@ struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
     ChartJacobian H = {}) {
     return m.inverse(H);
   }
+
+  static Eigen::Matrix<double, dimension, dimension> AdjointMap(const Class& m) {
+    // This assumes that the Class itself provides a member function `AdjointMap()`
+    // For dynamically-sized types (dimension == Eigen::Dynamic),
+    // m.AdjointMap() must return a gtsam::Matrix of the correct runtime dimensions.
+    return m.AdjointMap();
+  }
   /// @}
 };
 

gtsam/base/VectorSpace.h

@@ -35,7 +35,7 @@ struct VectorSpaceImpl {
       ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
     if (H1) *H1 = - Jacobian::Identity();
     if (H2) *H2 = Jacobian::Identity();
-    Class v = other-origin;
+    Class v = other - origin;
     return v.vector();
   }
 
@@ -82,12 +82,12 @@ struct VectorSpaceImpl {
     return -v;
   }
 
-  static LieAlgebra Hat(const TangentVector& v) {
+  static LieAlgebra Hat(const TangentVector& v) { return v; }
-    return v;
-  }
 
-  static TangentVector Vee(const LieAlgebra& X) {
+  static TangentVector Vee(const LieAlgebra& X) { return X; }
-    return X;
+
+  static Jacobian AdjointMap(const Class& /*m*/) {
+    return Jacobian::Identity();
   }
   /// @}
 };
@@ -118,7 +118,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
       ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
     if (H1) *H1 = - Eye(origin);
     if (H2) *H2 = Eye(other);
-    Class v = other-origin;
+    Class v = other - origin;
     return v.vector();
   }
 
@@ -141,8 +141,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
 
   static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
     Class result(v);
-    if (Hv)
+    if (Hv) *Hv = Eye(v);
-      *Hv = Eye(v);
     return result;
   }
 
@@ -165,6 +164,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
     return -v;
   }
 
+  static Eigen::MatrixXd AdjointMap(const Class& m) { return Eye(m); }
   /// @}
 };
 
@@ -273,8 +273,8 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
     if (H) (*H)[0] = 1.0;
     return v[0];
   }
+  // AdjointMap for ScalarTraits is inherited from VectorSpaceImpl<Scalar, 1>
   /// @}
-
 };
 
 } // namespace internal
@@ -352,6 +352,9 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
     if (H) *H = Jacobian::Identity();
     return m + Eigen::Map<const Fixed>(v.data());
   }
+
+  // AdjointMap for fixed-size Eigen matrices is inherited from
+  // internal::VectorSpaceImpl< Eigen::Matrix<double, M, N, ...> , M*N >
   /// @}
 };
 
@@ -425,7 +428,7 @@ struct DynamicTraits {
   static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) {
     if (H) *H = Eye(m);
     TangentVector result(GetDimension(m));
-    Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
+    Eigen::Map<Dynamic>(result.data(), m.rows(), m.cols()) = m;
     return result;
   }
 
@@ -461,6 +464,7 @@ struct DynamicTraits {
     return X;
   }
 
+  static Jacobian AdjointMap(const Dynamic& m) { return Eye(m); }
   /// @}
 
 };
@@ -506,4 +510,3 @@ private:
 };
 
 }  // namespace gtsam
-

gtsam/navigation/InvariantEKF.h

@@ -77,9 +77,9 @@ namespace gtsam {
      * @param Q Process noise covariance.
      */
     void predict(const G& U, const Covariance& Q) {
-      this->X_ = this->X_.compose(U);
+      this->X_ = traits<G>::Compose(this->X_, U);
-      // TODO(dellaert): traits<G>::AdjointMap should exist
+      const G U_inv = traits<G>::Inverse(U);
-      const Jacobian A = traits<G>::Inverse(U).AdjointMap();
+      const Jacobian A = traits<G>::AdjointMap(U_inv);
       // P_ is Covariance. A is Jacobian. Q is Covariance.
       // All are Eigen::Matrix<double,Dim,Dim>.
       this->P_ = A * this->P_ * A.transpose() + Q;