Merge remote-tracking branch 'origin/feature/ImprovedLie2' into feature/NavState_new_retract

Conflicts:
	gtsam/base/Lie.h

gtsam/base/Lie.h

@@ -75,57 +75,89 @@ 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,H1,H2);
-    if (H2) *H2 = (*H2) * D_g_v;
+    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,H1,H2);
+    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 * (*H1);
-    if (H2) *H2 = D_v_h * (*H2);
+    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;
-    Class g = Class::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
-    Class h = compose(g,H1,H2);
-    if (H2) *H2 = (*H2) * D_g_v;
+    Class g = Class::ChartAtOrigin::Retract(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;
   }
 
+  /// localCoordinates with optional derivatives
   TangentVector localCoordinates(const Class& g, //
       ChartJacobian H1, ChartJacobian H2 = boost::none) const {
-    Class h = between(g,H1,H2);
+    Class h = between(g); // derivatives inlined below
     Jacobian D_v_h;
     TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
-    if (H1) *H1 = D_v_h * (*H1);
-    if (H2) *H2 = D_v_h * (*H2);
+    if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
+    if (H2) *H2 = D_v_h;
     return v;
   }
 

gtsam/geometry/Pose3.cpp

@@ -157,7 +157,7 @@ Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
   return Expmap(xi, H);
 #else
   Matrix3 DR;
-  Rot3 R = Rot3::ChartAtOrigin::Retract(xi.head<3>(), H ? &DR : 0);
+  Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0);
   if (H) {
     *H = I_6x6;
     H->topLeftCorner<3,3>() = DR;
@@ -172,7 +172,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
   return Logmap(T, H);
 #else
   Matrix3 DR;
-  Vector3 omega = Rot3::ChartAtOrigin::Local(T.rotation(), H ? &DR : 0);
+  Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0);
   if (H) {
     *H = I_6x6;
     H->topLeftCorner<3,3>() = DR;

gtsam/geometry/tests/testPose3.cpp

@@ -471,6 +471,15 @@ TEST( Pose3, transformPose_to)
     EXPECT(assert_equal(expected, actual, 0.001));
 }
 
+/* ************************************************************************* */
+TEST(Pose3, Retract_LocalCoordinates)
+{
+  Vector6 d;
+  d << 1,2,3,4,5,6; d/=10;
+  R = Rot3::Retract(d.head<3>());
+  Pose3 t = Pose3::Retract(d);
+  EXPECT(assert_equal(d, Pose3::LocalCoordinates(t)));
+}
 /* ************************************************************************* */
 TEST(Pose3, retract_localCoordinates)
 {