From f9494679d27bdcd9a5b2f90efbacc30a0872e70e Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Mon, 22 Oct 2012 17:31:41 +0000
Subject: [PATCH] Added optional derivatives to 'inverse' and 'compose' for
 variable type 'PoseRTV'

---
 gtsam_unstable/dynamics/PoseRTV.cpp | 11 +++++++++--
 gtsam_unstable/dynamics/PoseRTV.h   | 13 +++++++++----
 2 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp
index 9fb2d4590..bf10e38b8 100644
--- a/gtsam_unstable/dynamics/PoseRTV.cpp
+++ b/gtsam_unstable/dynamics/PoseRTV.cpp
@@ -93,12 +93,19 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
 }
 
 /* ************************************************************************* */
-PoseRTV PoseRTV::inverse() const {
+PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
+PoseRTV PoseRTV::inverse(boost::optional<Matrix&> H1) const {
+  if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5);
   return PoseRTV(Rt_.inverse(), v_.inverse());
 }
 
 /* ************************************************************************* */
-PoseRTV PoseRTV::compose(const PoseRTV& p) const {
+PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); }
+PoseRTV PoseRTV::compose(const PoseRTV& p,
+    boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+  if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5);
+  if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5);
   return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_));
 }
 
diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h
index b270a2ae9..2dd002010 100644
--- a/gtsam_unstable/dynamics/PoseRTV.h
+++ b/gtsam_unstable/dynamics/PoseRTV.h
@@ -88,12 +88,17 @@ public:
   static PoseRTV Expmap(const Vector& v);
   static Vector Logmap(const PoseRTV& p);
 
-  PoseRTV inverse() const;
-
-  PoseRTV compose(const PoseRTV& p) const;
-
   static PoseRTV identity() { return PoseRTV(); }
 
+  /** Derivatives calculated numerically */
+  PoseRTV inverse(boost::optional<Matrix&> H1=boost::none) const;
+
+  /** Derivatives calculated numerically */
+  PoseRTV compose(const PoseRTV& p,
+      boost::optional<Matrix&> H1=boost::none,
+      boost::optional<Matrix&> H2=boost::none) const;
+
+
   /** Derivatives calculated numerically */
   PoseRTV between(const PoseRTV& p,
       boost::optional<Matrix&> H1=boost::none,