LieGroupChart makes use of Exponential map (and its inverse) around identity

gtsam/base/concepts.h

@@ -278,6 +278,29 @@ private:
   T g, h;
 };
 
+/**
+ * A Lie Group Chart
+ * Creates Local/Retract from exponential map and its inverse
+ * Assumes Expmap and Logmap defined in Derived
+ * TODO: Can we do this with a single Derived argument ?
+ */
+template<typename Derived, typename T, typename TangentVector>
+struct LieGroupChart {
+
+  /// retract, composes with Exmpap around identity
+  static T Retract(const T& p, const TangentVector& omega) {
+    // TODO needs to be manifold::compose, with derivatives
+    return group::compose(p, Derived::Expmap(omega));
+  }
+
+  /// local is our own, as there is a slight bug in Eigen
+  static TangentVector Local(const T& q1, const T& q2) {
+    // TODO needs to be manifold::between, with derivatives
+    return Derived::Logmap(group::between(q1, q2));
+  }
+
+};
+
 template<typename T>
 class IsVectorSpace: public IsLieGroup<T> {
 public:

gtsam/geometry/Quaternion.h

@@ -21,7 +21,10 @@ namespace gtsam {
 
 /// Chart for Eigen Quaternions
 template<typename _Scalar, int _Options>
-struct MakeQuaternionChart {
+struct MakeQuaternionChart: LieGroupChart<
+    MakeQuaternionChart<_Scalar, _Options>,
+    Eigen::Quaternion<_Scalar, _Options>,
+    Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> > {
 
   // required
   typedef Eigen::Quaternion<_Scalar, _Options> ManifoldType;

gtsam/geometry/SO3.cpp

@@ -21,7 +21,7 @@
 
 namespace gtsam {
 
-SO3 SO3Chart::Expmap(const double& theta, const Vector3& axis) {
+SO3 Rodrigues(const double& theta, const Vector3& axis) {
   using std::cos;
   using std::sin;
 
@@ -44,6 +44,16 @@ SO3 SO3Chart::Expmap(const double& theta, const Vector3& axis) {
   return R;
 }
 
+/// simply convert omega to axis/angle representation
+SO3 SO3Chart::Expmap(const Eigen::Ref<const Vector3>& omega) {
+  if (omega.isZero())
+    return SO3::Identity();
+  else {
+    double angle = omega.norm();
+    return Rodrigues(angle, omega / angle);
+  }
+}
+
 Vector3 SO3Chart::Logmap(const SO3& R) {
 
   // note switch to base 1

gtsam/geometry/SO3.h

@@ -11,7 +11,7 @@
 
 /**
  * @file    SO3.h
- * @brief   3*3 matrix representation o SO(3)
+ * @brief   3*3 matrix representation of SO(3)
  * @author  Frank Dellaert
  * @date    December 2014
  */
@@ -48,37 +48,18 @@ public:
   }
 };
 
-/// Default Chart for SO3
+/**
-struct SO3Chart {
+ * Chart for SO3 comprising of exponential map and its inverse (log-map)
+ */
+struct SO3Chart: LieGroupChart<SO3Chart,SO3,Vector3> {
 
   typedef SO3 ManifoldType;
 
-  /// Exponential map given axis/angle representation of Lie algebra
+  /// Exponential map
-  static SO3 Expmap(const double& theta, const Vector3& w);
+  static SO3 Expmap(const Eigen::Ref<const Vector3>& omega);
-
-  /// Exponential map, simply be converting omega to axis/angle representation
-  static SO3 Expmap(const Eigen::Ref<const Vector3>& omega) {
-    if (omega.isZero())
-      return SO3::Identity();
-    else {
-      double angle = omega.norm();
-      return Expmap(angle, omega / angle);
-    }
-  }
-
-  /// retract, simply be converting omega to AngleAxis
-  static SO3 Retract(const SO3& p, const Eigen::Ref<const Vector3>& omega) {
-    return p * Expmap(omega);
-  }
 
   /// We use our own Logmap, as there is a slight bug in Eigen
   static Vector3 Logmap(const SO3& R);
-
-  /// local is our own, as there is a slight bug in Eigen
-  static Vector3 Local(const SO3& q1, const SO3& q2) {
-    return Logmap(q1.inverse() * q2);
-  }
-
 };
 
 #define SO3_TEMPLATE