Working compose/between/inverse derivatives

gtsam/base/ProductLieGroup.h

@@ -23,11 +23,10 @@
 
 namespace gtsam {
 
-/// Template to construct the product Lie group of two other Lie groups, G and H
+/// Template to construct the product Lie group of two other Lie groups
-/// Assumes manifold structure from G and H, and binary constructor
+/// Assumes Lie group structure for G and H
 template<typename G, typename H>
-class ProductLieGroup: public std::pair<G, H>, public LieGroup<
+class ProductLieGroup: public std::pair<G, H> {
-    ProductLieGroup<G, H>, traits<G>::dimension + traits<H>::dimension> {
   BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
   BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
   typedef std::pair<G, H> Base;
@@ -58,9 +57,15 @@ public:
   ProductLieGroup inverse() const {
     return ProductLieGroup(this->first.inverse(), this->second.inverse());
   }
+  ProductLieGroup compose(const ProductLieGroup& g) const {
+    return (*this) * g;
+  }
+  ProductLieGroup between(const ProductLieGroup& g) const {
+    return this->inverse() * g;
+  }
   /// @}
 
-  /// @name Manifold (but with derivatives)
+  /// @name Manifold
   /// @{
   enum {dimension = dimension1 + dimension2};
   inline static size_t Dim() {return dimension;}
@@ -68,26 +73,74 @@ public:
 
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  static ProductLieGroup Retract(const TangentVector& v) {
+    return ProductLieGroup::ChartAtOrigin::Retract(v);
+  }
+  static TangentVector LocalCoordinates(const ProductLieGroup& g) {
+    return ProductLieGroup::ChartAtOrigin::Local(g);
+  }
+  ProductLieGroup retract(const TangentVector& v) const {
+    return compose(ProductLieGroup::ChartAtOrigin::Retract(v));
+  }
+  TangentVector localCoordinates(const ProductLieGroup& g) const {
+    return ProductLieGroup::ChartAtOrigin::Local(between(g));
+  }
   /// @}
 
   /// @name Lie Group
   /// @{
-  Eigen::Matrix<double,dimension,dimension> AdjointMap() const {
+protected:
-    Eigen::Matrix<double,dimension,dimension> A;
+  typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
-    A.setIdentity();
+  typedef Eigen::Matrix<double, dimension1, dimension1> Jacobian1;
-    throw std::runtime_error("ProductLieGroup::derivatives not implemented yet");
+  typedef Eigen::Matrix<double, dimension2, dimension2> Jacobian2;
-    //    A.template topLeftCorner<dimension1, dimension1>() = this->first.AdjointMap();
+
-    //    A.template bottomRightCorner<dimension2, dimension2>() = this->second.AdjointMap();
+public:
-    return A;
+  ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
+      ChartJacobian H2 = boost::none) const {
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
+    H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
+    if (H1) {
+      H1->setZero();
+      H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
+    if (H2) *H2 = Jacobian::Identity();
+    return ProductLieGroup(g,h);
+  }
+  ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
+      ChartJacobian H2 = boost::none) const {
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
+    H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
+    if (H1) {
+      H1->setZero();
+      H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
+    if (H2) *H2 = Jacobian::Identity();
+    return ProductLieGroup(g,h);
+  }
+  ProductLieGroup inverse(ChartJacobian D) const {
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Inverse(this->first, D ? &D_g_first : 0);
+    H h = traits<H>::Inverse(this->second, D ? &D_h_second : 0);
+    if (D) {
+      D->setZero();
+      D->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      D->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
+    return ProductLieGroup(g,h);
   }
   static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
-    if (Hv) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet");
+    if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet");
     G g = traits<G>::Expmap(v.template head<dimension1>());
     H h = traits<H>::Expmap(v.template tail<dimension2>());
     return ProductLieGroup(g,h);
   }
   static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
-    if (Hp) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet");
+    if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet");
     typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first);
     typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second);
     TangentVector v;
@@ -102,8 +155,37 @@ public:
       return Expmap(v, Hv);
     }
   };
-  using LieGroup<ProductLieGroup,dimension>::inverse; // with derivative
+  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

gtsam_unstable/dynamics/tests/testPoseRTV.cpp

@@ -120,6 +120,43 @@ TEST( testPoseRTV, dynamics_identities ) {
 }
 
 
+/* ************************************************************************* */
+PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); }
+TEST( testPoseRTV, compose ) {
+  PoseRTV state1(pt, rot, vel), state2 = state1;
+
+  Matrix actH1, actH2;
+  state1.compose(state2, actH1, actH2);
+  Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
+  Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
+  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); }
+TEST( testPoseRTV, between ) {
+  PoseRTV state1(pt, rot, vel), state2 = state1;
+
+  Matrix actH1, actH2;
+  state1.between(state2, actH1, actH2);
+  Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
+  Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
+  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); }
+TEST( testPoseRTV, inverse ) {
+  PoseRTV state1(pt, rot, vel);
+
+  Matrix actH1;
+  state1.inverse(actH1);
+  Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
+  EXPECT(assert_equal(numericH1, actH1, tol));
+}
+
 /* ************************************************************************* */
 double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
 TEST( testPoseRTV, range ) {