Added retract/logmap and commented more clearly

gtsam/base/Lie.h

@@ -75,38 +75,71 @@ struct LieGroup {
     return derived().inverse();
   }
 
+  /// expmap as required by manifold concept
+  /// Applies exponential map to v and composes with *this
   Class expmap(const TangentVector& v) const {
     return compose(Class::Expmap(v));
   }
 
+  /// logmap as required by manifold concept
+  /// Applies logarithmic map to group element that takes *this to g
   TangentVector logmap(const Class& g) const {
     return Class::Logmap(between(g));
   }
 
+  /// expmap with optional derivatives
+  Class expmap(const TangentVector& v, //
+      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
+    Jacobian D_g_v;
+    Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
+    Class h = compose(g); // derivatives inlined below
+    if (H1) *H1 = g.inverse().AdjointMap();
+    if (H2) *H2 = D_g_v;
+    return h;
+  }
+
+  /// logmap with optional derivatives
+  TangentVector logmap(const Class& g, //
+      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
+    Class h = between(g); // derivatives inlined below
+    Jacobian D_v_h;
+    TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
+    if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
+    if (H2) *H2 = D_v_h;
+    return v;
+  }
+
+  /// Retract at origin: possible in Lie group because it has an identity
   static Class Retract(const TangentVector& v) {
     return Class::ChartAtOrigin::Retract(v);
   }
 
+  /// LocalCoordinates at origin: possible in Lie group because it has an identity
   static TangentVector LocalCoordinates(const Class& g) {
     return Class::ChartAtOrigin::Local(g);
   }
 
+  /// Retract at origin with optional derivative
   static Class Retract(const TangentVector& v, ChartJacobian H) {
     return Class::ChartAtOrigin::Retract(v,H);
   }
 
+  /// LocalCoordinates at origin with optional derivative
   static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) {
     return Class::ChartAtOrigin::Local(g,H);
   }
 
+  /// retract as required by manifold concept: applies v at *this
   Class retract(const TangentVector& v) const {
     return compose(Class::ChartAtOrigin::Retract(v));
   }
 
+  /// localCoordinates as required by manifold concept: finds tangent vector between *this and g
   TangentVector localCoordinates(const Class& g) const {
     return Class::ChartAtOrigin::Local(between(g));
   }
 
+  /// retract with optional derivatives
   Class retract(const TangentVector& v, //
       ChartJacobian H1, ChartJacobian H2 = boost::none) const {
     Jacobian D_g_v;
@@ -117,6 +150,7 @@ struct LieGroup {
     return h;
   }
 
+  /// localCoordinates with optional derivatives
   TangentVector localCoordinates(const Class& g, //
       ChartJacobian H1, ChartJacobian H2 = boost::none) const {
     Class h = between(g); // derivatives inlined below