From d7d35876f1ee67dc928414c298304f3e14e34a92 Mon Sep 17 00:00:00 2001
From: Jing Dong <jdong@gatech.edu>
Date: Mon, 3 Oct 2016 19:11:44 -0400
Subject: [PATCH 1/2] jacobians for logmap and expmap

---
 gtsam/base/ProductLieGroup.h | 22 ++++++++++++++++------
 tests/testLie.cpp            | 27 +++++++++++++++++++++++++++
 2 files changed, 43 insertions(+), 6 deletions(-)

diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h
index 463b5f5d9..87ead88f0 100644
--- a/gtsam/base/ProductLieGroup.h
+++ b/gtsam/base/ProductLieGroup.h
@@ -138,17 +138,27 @@ public:
     return ProductLieGroup(g,h);
   }
   static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
-    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>());
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Expmap(v.template head<dimension1>(), Hv ? &D_g_first : 0);
+    H h = traits<H>::Expmap(v.template tail<dimension2>(), Hv ? &D_h_second : 0);
+    if (Hv) {
+      Hv->setZero();
+      Hv->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      Hv->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
     return ProductLieGroup(g,h);
   }
   static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
-    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);
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first, Hp ? &D_g_first : 0);
+    typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second, Hp ? &D_h_second : 0);
     TangentVector v;
     v << v1, v2;
+    if (Hp) {
+      Hp->setZero();
+      Hp->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      Hp->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
     return v;
   }
   ProductLieGroup expmap(const TangentVector& v) const {
diff --git a/tests/testLie.cpp b/tests/testLie.cpp
index a134a899c..2d8a0b975 100644
--- a/tests/testLie.cpp
+++ b/tests/testLie.cpp
@@ -102,6 +102,33 @@ TEST( testProduct, inverse ) {
   EXPECT(assert_equal(numericH1, actH1, tol));
 }
 
+/* ************************************************************************* */
+Product expmap_proxy(const Vector5& vec) {
+  return Product::Expmap(vec);
+}
+TEST( testProduct, Expmap ) {
+  Vector5 vec;
+  vec << 1, 2, 0.1, 0.2, 0.3;
+
+  Matrix actH;
+  Product::Expmap(vec, actH);
+  Matrix numericH = numericalDerivative11(expmap_proxy, vec);
+  EXPECT(assert_equal(numericH, actH, tol));
+}
+
+/* ************************************************************************* */
+Vector5 logmap_proxy(const Product& p) {
+  return Product::Logmap(p);
+}
+TEST( testProduct, Logmap ) {
+  Product state(Point2(1, 2), Pose2(3, 4, 5));
+
+  Matrix actH;
+  Product::Logmap(state, actH);
+  Matrix numericH = numericalDerivative11(logmap_proxy, state);
+  EXPECT(assert_equal(numericH, actH, tol));
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;

From 5fa4abf99cb89c0c9af9dc198b49f5e936a4f6f4 Mon Sep 17 00:00:00 2001
From: Jing Dong <jdong@gatech.edu>
Date: Thu, 8 Dec 2016 15:19:18 -0500
Subject: [PATCH 2/2] fix optional jacobians

---
 gtsam/base/VectorSpace.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h
index 5456ae7f5..43644b5c4 100644
--- a/gtsam/base/VectorSpace.h
+++ b/gtsam/base/VectorSpace.h
@@ -138,14 +138,14 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
   }
 
   static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
-      ChartJacobian H2) {
+      ChartJacobian H2 = boost::none) {
     if (H1) *H1 = Eye(v1);
     if (H2) *H2 = Eye(v2);
     return v1 + v2;
   }
 
   static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
-      ChartJacobian H2) {
+      ChartJacobian H2 = boost::none) {
     if (H1) *H1 = - Eye(v1);
     if (H2) *H2 =   Eye(v2);
     return v2 - v1;