The great Lie group heist: LieGroup CRTP class that defines both Manifold and Lie Group methods for a class, using AdjointMap as its main weapon. For the retract/localCoordinates, a ChartAtOrigin struct needs to be defined that can be switched at compile time (example Pose2). Rot3, Pose3 soon to follow.

gtsam/base/Lie.h

@@ -24,6 +24,67 @@
 
 namespace gtsam {
 
+/// A CRTP helper class that implements Lie group methods
+/// Prerequisites: methods operator*, inverse, and AdjointMap, as well as a
+/// ChartAtOrigin struct that will be used to define the manifold Chart
+/// To use, simply derive, but also say "using LieGroup<Class,N>::inverse"
+/// For derivative math, see doc/math.pdf
+template <class Class, int N>
+struct LieGroup {
+
+  BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic,
+      "LieGroup not yet specialized for dynamically sized types.");
+
+  enum { dimension = N };
+  typedef OptionalJacobian<N, N> ChartJacobian;
+  typedef Eigen::Matrix<double, N, 1> TangentVector;
+
+  const Class & derived() const {
+    return static_cast<const Class&>(*this);
+  }
+
+  Class compose(const Class& g) const {
+    return derived() * g;
+  }
+
+  Class between(const Class& g) const {
+    return derived().inverse() * g;
+  }
+
+  Class compose(const Class& g, ChartJacobian H1,
+      ChartJacobian H2 = boost::none) const {
+    if (H1) *H1 = g.inverse().AdjointMap();
+    if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
+    return derived() * g;
+  }
+
+  Class between(const Class& g, ChartJacobian H1,
+      ChartJacobian H2 = boost::none) const {
+    Class result = derived().inverse() * g;
+    if (H1) *H1 = - result.inverse().AdjointMap();
+    if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
+    return result;
+  }
+
+  Class inverse(ChartJacobian H) const {
+    if (H) *H = - derived().AdjointMap();
+    return derived().inverse();
+  }
+
+  Class retract(const TangentVector& v, //
+      ChartJacobian H1 = boost::none, //
+      ChartJacobian H2 = boost::none) const {
+    return compose(Class::ChartAtOrigin::Retract(v),H1,H2);
+  }
+
+  TangentVector localCoordinates(const Class& g, //
+      ChartJacobian H1 = boost::none, //
+      ChartJacobian H2 = boost::none) const {
+    return Class::ChartAtOrigin::Local(between(g,H1,H2));
+  }
+
+};
+
 /// tag to assert a type is a Lie group
 struct lie_group_tag: public manifold_tag, public group_tag {};
 

gtsam/geometry/Pose2.cpp

@@ -92,25 +92,20 @@ Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) {
 }
 
 /* ************************************************************************* */
-Pose2 Pose2::retract(const Vector& v, OptionalJacobian<3, 3> Hthis,
+Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) {
-    OptionalJacobian<3, 3> Hv) const {
-  if (Hthis || Hv) CONCEPT_NOT_IMPLEMENTED;
 #ifdef SLOW_BUT_CORRECT_EXPMAP
-  return compose(Expmap(v));
+  return Expmap(v, H);
 #else
-  assert(v.size() == 3);
+  if (H) *H = Matrix3::Identity();
-  return compose(Pose2(v[0], v[1], v[2]));
+  return Pose2(v[0], v[1], v[2]);
 #endif
 }
-
 /* ************************************************************************* */
-Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis,
+Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) {
-    OptionalJacobian<3, 3> Hother) const {
-  if (Hthis || Hother) CONCEPT_NOT_IMPLEMENTED;
 #ifdef SLOW_BUT_CORRECT_EXPMAP
-  return Logmap(between(p2));
+  return Logmap(r, H);
 #else
-  Pose2 r = between(p2);
+  if (H) *H = Matrix3::Identity();
   return Vector3(r.x(), r.y(), r.theta());
 #endif
 }
@@ -189,8 +184,7 @@ Matrix3 Pose2::dexpInvL(const Vector3& v) {
 }
 
 /* ************************************************************************* */
-Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const {
+Pose2 Pose2::inverse() const {
-  if (H1) *H1 = -AdjointMap();
   return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
 }
 
@@ -208,16 +202,6 @@ Point2 Pose2::transform_to(const Point2& point,
   return q;
 }
 
-/* ************************************************************************* */
-// see doc/math.lyx, SE(2) section
-Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
-    OptionalJacobian<3,3> H2) const {
-  // TODO: inline and reuse?
-  if(H1) *H1 = p2.inverse().AdjointMap();
-  if(H2) *H2 = I_3x3;
-  return (*this)*p2;
-}
-
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
 Point2 Pose2::transform_from(const Point2& p,
@@ -233,40 +217,6 @@ Point2 Pose2::transform_from(const Point2& p,
   return q + t_;
 }
 
-/* ************************************************************************* */
-Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
-    OptionalJacobian<3,3> H2) const {
-  // get cosines and sines from rotation matrices
-  const Rot2& R1 = r_, R2 = p2.r();
-  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
-
-  // Assert that R1 and R2 are normalized
-  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
-
-  // Calculate delta rotation = between(R1,R2)
-  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
-  Rot2 R(Rot2::atan2(s,c)); // normalizes
-
-  // Calculate delta translation = unrotate(R1, dt);
-  Point2 dt = p2.t() - t_;
-  double x = dt.x(), y = dt.y();
-  // t = R1' * (t2-t1)
-  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
-
-  // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
-  if (H1) {
-    double dt1 = -s2 * x + c2 * y;
-    double dt2 = -c2 * x - s2 * y;
-    *H1 <<
-        -c,  -s,  dt1,
-        s,  -c,  dt2,
-        0.0, 0.0,-1.0;
-  }
-  if (H2) *H2 = I_3x3;
-
-  return Pose2(R,t);
-}
-
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Point2& point,
     OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {

gtsam/geometry/Pose2.h

@@ -31,7 +31,7 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Pose2 {
+class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
 
 public:
 
@@ -40,13 +40,12 @@ public:
   typedef Point2 Translation;
 
 private:
+
   Rot2 r_;
   Point2 t_;
 
 public:
 
-  enum { dimension = 3 };
-
   /// @name Standard Constructors
   /// @{
 
@@ -106,52 +105,23 @@ public:
   /// identity for group operation
   inline static Pose2 identity() { return Pose2(); }
 
-  /// inverse transformation with derivatives
+  /// inverse
-  Pose2 inverse(OptionalJacobian<3, 3> H1=boost::none) const;
+  Pose2 inverse() const;
-
-  /// compose this transformation onto another (first *this and then p2)
-  Pose2 compose(const Pose2& p2,
-      OptionalJacobian<3, 3> H1 = boost::none,
-      OptionalJacobian<3, 3> H2 = boost::none) const;
 
   /// compose syntactic sugar
   inline Pose2 operator*(const Pose2& p2) const {
     return Pose2(r_*p2.r(), t_ + r_*p2.t());
   }
 
-  /**
-   * Return relative pose between p1 and p2, in p1 coordinate frame
-   */
-  Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none,
-      OptionalJacobian<3,3> H = boost::none) const;
-
-  /// @}
-  /// @name Manifold
-  /// @{
-
-  /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes
-  inline static size_t Dim() { return 3; }
-
-  /// Dimensionality of tangent space = 3 DOF
-  inline size_t dim() const { return 3; }
-
-  /// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose
-  Pose2 retract(const Vector& v, OptionalJacobian<3, 3> Hthis =
-      boost::none, OptionalJacobian<3, 3> Hv = boost::none) const;
-
-  /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
-  Vector localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis =
-      boost::none, OptionalJacobian<3, 3> Hother = boost::none) const;
-
   /// @}
   /// @name Lie Group
   /// @{
 
   ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
-  static Pose2 Expmap(const Vector& xi, OptionalJacobian<3, 3> H = boost::none);
+  static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none);
 
   ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
-  static Vector3 Logmap(const Pose2& p, OptionalJacobian<3, 3> H = boost::none);
+  static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
 
   /**
    * Calculate Adjoint map
@@ -189,6 +159,13 @@ public:
   /// Left-trivialized derivative inverse of the exponential map
   static Matrix3 dexpInvL(const Vector3& v);
 
+  // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
+  struct ChartAtOrigin {
+    static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
+    static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
+  };
+
+  using LieGroup<Pose2, 3>::inverse; // version with derivative
 
   /// @}
   /// @name Group Action on Point2

gtsam/geometry/Rot2.cpp

@@ -60,47 +60,7 @@ Rot2& Rot2::normalize() {
 }
 
 /* ************************************************************************* */
-Rot2 Rot2::compose(const Rot2& R, OptionalJacobian<1, 1> H1,
+Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) {
-    OptionalJacobian<1, 1> H2) const {
-  if (H1)
-    *H1 = I_1x1;
-  if (H2)
-    *H2 = I_1x1;
-  return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
-}
-
-/* ************************************************************************* */
-Rot2 Rot2::between(const Rot2& R, OptionalJacobian<1, 1> H1,
-    OptionalJacobian<1, 1> H2) const {
-  if (H1)
-    *H1 = -I_1x1;
-  if (H2)
-    *H2 = I_1x1;
-  return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
-}
-
-/* ************************************************************************* */
-Rot2 Rot2::retract(const Vector& v, OptionalJacobian<1, 1> H1,
-    OptionalJacobian<1, 1> H2) const {
-  if (H1)
-    *H1 = I_1x1;
-  if (H2)
-    *H2 = I_1x1;
-  return *this * Expmap(v);
-}
-
-/* ************************************************************************* */
-Vector1 Rot2::localCoordinates(const Rot2& t2, OptionalJacobian<1, 1> H1,
-    OptionalJacobian<1, 1> H2) const {
-  if (H1)
-    *H1 = -I_1x1;
-  if (H2)
-    *H2 = I_1x1;
-  return Logmap(between(t2));
-}
-
-/* ************************************************************************* */
-Rot2 Rot2::Expmap(const Vector& v, OptionalJacobian<1, 1> H) {
   if (H)
     *H = I_1x1;
   if (zero(v))

gtsam/geometry/Rot2.h

@@ -30,25 +30,16 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class GTSAM_EXPORT Rot2 {
+  class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
-
-  public:
-    /** get the dimension by the type */
-    static const size_t dimension = 1;
-
-  private:
 
     /** we store cos(theta) and sin(theta) */
     double c_, s_;
 
-
     /** normalize to make sure cos and sin form unit vector */
     Rot2& normalize();
 
     /** private constructor from cos/sin */
-    inline Rot2(double c, double s) :
+    inline Rot2(double c, double s) : c_(c), s_(s) {}
-        c_(c), s_(s) {
-    }
 
   public:
 
@@ -56,14 +47,10 @@ namespace gtsam {
     /// @{
 
     /** default constructor, zero rotation */
-    Rot2() :
+    Rot2() : c_(1.0), s_(0.0) {}
-        c_(1.0), s_(0.0) {
-    }
 
     /// Constructor from angle in radians == exponential map at identity
-    Rot2(double theta) :
+    Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
-        c_(cos(theta)), s_(sin(theta)) {
-    }
 
     /// Named constructor from angle in radians
     static Rot2 fromAngle(double theta) {
@@ -110,59 +97,26 @@ namespace gtsam {
     inline static Rot2 identity() {  return Rot2(); }
 
     /** The inverse rotation - negative angle */
-    Rot2 inverse(OptionalJacobian<1,1> H = boost::none) const {
+    Rot2 inverse() const { return Rot2(c_, -s_);}
-      if (H) *H = -I_1x1;
-      return Rot2(c_, -s_);
-    }
 
     /** Compose - make a new rotation by adding angles */
     Rot2 operator*(const Rot2& R) const {
       return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
     }
 
-    /** Compose - make a new rotation by adding angles */
-    Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
-        boost::none, OptionalJacobian<1,1> H2 = boost::none) const;
-
-    /** Between using the default implementation */
-    Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
-        boost::none, OptionalJacobian<1,1> H2 = boost::none) const;
-
-    /// @}
-    /// @name Manifold
-    /// @{
-
-    /// dimension of the variable - used to autodetect sizes
-    inline static size_t Dim() {
-      return dimension;
-    }
-
-    /// Dimensionality of the tangent space, DOF = 1
-    inline size_t dim() const {
-      return dimension;
-    }
-
-    /// Updates a with tangent space delta
-    Rot2 retract(const Vector& v, OptionalJacobian<1, 1> H1 = boost::none,
-        OptionalJacobian<1, 1> H2 = boost::none) const;
-
-    /// Returns inverse retraction
-    Vector1 localCoordinates(const Rot2& t2, OptionalJacobian<1, 1> H1 =
-        boost::none, OptionalJacobian<1, 1> H2 = boost::none) const;
-
     /// @}
     /// @name Lie Group
     /// @{
 
+    /// Exponential map at identity - create a rotation from canonical coordinates
+    static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none);
+
+    /// Log map at identity - return the canonical coordinates of this rotation
+    static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
+
     /** Calculate Adjoint map */
     Matrix1 AdjointMap() const { return I_1x1; }
 
-    ///Exponential map at identity - create a rotation from canonical coordinates
-    static Rot2 Expmap(const Vector& v, OptionalJacobian<1, 1> H = boost::none);
-
-    ///Log map at identity - return the canonical coordinates of this rotation
-    static Vector1 Logmap(const Rot2& r, OptionalJacobian<1, 1> H = boost::none);
-
     /// Left-trivialized derivative of the exponential map
     static Matrix dexpL(const Vector& v) {
       return ones(1);
@@ -173,6 +127,18 @@ namespace gtsam {
       return ones(1);
     }
 
+    // Chart at origin simply uses exponential map and its inverse
+    struct ChartAtOrigin {
+      static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) {
+        return Expmap(v, H);
+      }
+      static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) {
+        return Logmap(r, H);
+      }
+    };
+
+    using LieGroup<Rot2, 1>::inverse; // version with derivative
+
     /// @}
     /// @name Group Action on Point2
     /// @{