Fixed product retract/localCoordinates and corresponding tests

gtsam/base/ProductLieGroup.h

@@ -74,17 +74,21 @@ public:
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
-  static ProductLieGroup Retract(const TangentVector& v) {
+  ProductLieGroup retract(const TangentVector& v, //
-    return ProductLieGroup::ChartAtOrigin::Retract(v);
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
+    if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet");
+    G g = traits<G>::Retract(this->first, v.template head<dimension1>());
+    H h = traits<H>::Retract(this->second, v.template tail<dimension2>());
+    return ProductLieGroup(g,h);
   }
-  static TangentVector LocalCoordinates(const ProductLieGroup& g) {
+  TangentVector localCoordinates(const ProductLieGroup& g, //
-    return ProductLieGroup::ChartAtOrigin::Local(g);
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
-  }
+    if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet");
-  ProductLieGroup retract(const TangentVector& v) const {
+    typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
-    return compose(ProductLieGroup::ChartAtOrigin::Retract(v));
+    typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second);
-  }
+    TangentVector v;
-  TangentVector localCoordinates(const ProductLieGroup& g) const {
+    v << v1, v2;
-    return ProductLieGroup::ChartAtOrigin::Local(between(g));
+    return v;
   }
   /// @}
 
@@ -147,51 +151,19 @@ public:
     v << v1, v2;
     return v;
   }
-  struct ChartAtOrigin {
-    static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) {
-      return Logmap(m, Hm);
-    }
-    static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) {
-      return Expmap(v, Hv);
-    }
-  };
   ProductLieGroup expmap(const TangentVector& v) const {
     return compose(ProductLieGroup::Expmap(v));
   }
   TangentVector logmap(const ProductLieGroup& g) const {
     return ProductLieGroup::Logmap(between(g));
   }
-  static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) {
-    return ProductLieGroup::ChartAtOrigin::Retract(v,H1);
-  }
-  static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) {
-    return ProductLieGroup::ChartAtOrigin::Local(g,H1);
-  }
-  ProductLieGroup retract(const TangentVector& v, //
-      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
-    Jacobian D_g_v;
-    ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
-    ProductLieGroup h = compose(g,H1,H2);
-    if (H2) *H2 = (*H2) * D_g_v;
-    return h;
-  }
-  TangentVector localCoordinates(const ProductLieGroup& g, //
-      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
-    ProductLieGroup h = between(g,H1,H2);
-    Jacobian D_v_h;
-    TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
-    if (H1) *H1 = D_v_h * (*H1);
-    if (H2) *H2 = D_v_h * (*H2);
-    return v;
-  }
   /// @}
 
 };
 
 // Define any direct product group to be a model of the multiplicative Group concept
 template<typename G, typename H>
-struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<
+struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<ProductLieGroup<G, H> > {};
-    ProductLieGroup<G, H> > {
+
-};
 } // namespace gtsam
 

gtsam_unstable/dynamics/tests/testPoseRTV.cpp

@@ -86,21 +86,23 @@ TEST( testPoseRTV, Lie ) {
 
   Vector delta(9);
   delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3;
-  Rot3 rot2 = rot.retract(repeat(3, 0.1));
+  Pose3 pose2 = Pose3(rot, pt).retract(delta.head<6>());
-  Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4);
+  Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3);
-  Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3);
+  PoseRTV state2(pose2.translation(), pose2.rotation(), vel2);
-  PoseRTV state2(pt2, rot2, vel2);
+  EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), tol));
-  EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1));
+  EXPECT(assert_equal(delta, state1.localCoordinates(state2), tol));
-  EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1));
-  EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1));
 
   // roundtrip from state2 to state3 and back
   PoseRTV state3 = state2.retract(delta);
-  EXPECT(assert_equal(delta, state2.localCoordinates(state3), 1e-1));
+  EXPECT(assert_equal(delta, state2.localCoordinates(state3), tol));
 
-  // roundtrip from state3 to state4 and back, with expmap
+  // roundtrip from state3 to state4 and back, with expmap.
   PoseRTV state4 = state3.expmap(delta);
-  EXPECT(assert_equal(delta, state3.logmap(state4), 1e-1));
+  EXPECT(assert_equal(delta, state3.logmap(state4), tol));
+
+  // For the expmap/logmap (not necessarily retract/local) -delta goes other way
+  EXPECT(assert_equal(state3, (PoseRTV)state4.expmap(-delta), tol));
+  EXPECT(assert_equal(delta, -state4.logmap(state3), tol));
 }
 
 /* ************************************************************************* */

tests/testLie.cpp

@@ -54,9 +54,9 @@ TEST(Lie, ProductLieGroup) {
   Vector5 d;
   d << 1, 2, 0.1, 0.2, 0.3;
   Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3)));
-  Product pair2 = pair1.retract(d);
+  Product pair2 = pair1.expmap(d);
   EXPECT(assert_equal(expected, pair2, 1e-9));
-  EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9));
+  EXPECT(assert_equal(d, pair1.logmap(pair2), 1e-9));
 }
 
 /* ************************************************************************* */