From 981082e2a48687e0d3a0183a6505bda58667f26b Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 24 May 2010 05:40:59 +0000
Subject: [PATCH] Jacobian versions of compose and between

---
 cpp/Point2.h  | 17 ++++++++++++++++-
 cpp/Pose2.cpp |  8 ++++++++
 cpp/Pose2.h   |  9 ++++++---
 3 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/cpp/Point2.h b/cpp/Point2.h
index e3efdddcb..c03df7c44 100644
--- a/cpp/Point2.h
+++ b/cpp/Point2.h
@@ -88,7 +88,13 @@ namespace gtsam {
   inline Vector logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
 
   /** "Compose", just adds the coordinates of two points. */
-  inline Point2 compose(const Point2& p1, const Point2& p0) { return p0+p1; }
+  inline Point2 compose(const Point2& p1, const Point2& p2) { return p1+p2; }
+  inline Point2 compose(const Point2& p1, const Point2& p2,
+      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
+    if(H1) *H1 = eye(2);
+    if(H2) *H2 = eye(2);
+    return compose(p1, p2);
+  }
   inline Matrix Dcompose1(const Point2& p1, const Point2& p0) {
     return Matrix_(2,2,
         1.0, 0.0,
@@ -98,6 +104,15 @@ namespace gtsam {
         1.0, 0.0,
         0.0, 1.0); }
 
+  /** "Between", subtracts point coordinates */
+  inline Point2 between(const Point2& p1, const Point2& p2) { return p2-p1; }
+  inline Point2 between(const Point2& p1, const Point2& p2,
+      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
+    if(H1) *H1 = -eye(2);
+    if(H2) *H2 = eye(2);
+    return between(p1, p2);
+  }
+
   /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
   inline Point2 inverse(const Point2& p) { return Point2(-p.x(), -p.y()); }
 
diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp
index 9ffec1a18..7668256b3 100644
--- a/cpp/Pose2.cpp
+++ b/cpp/Pose2.cpp
@@ -121,6 +121,14 @@ namespace gtsam {
   /* ************************************************************************* */
   // see doc/math.lyx, SE(2) section
 
+  Pose2 compose(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
+      boost::optional<Matrix&> H2) {
+    // TODO: inline and reuse?
+    if(H1) *H1 = AdjointMap(inverse(p2));
+    if(H2) *H2 = I3;
+    return p1*p2;
+  }
+
   Matrix Dcompose1(const Pose2& p1, const Pose2& p2) {
 		return AdjointMap(inverse(p2));
   }
diff --git a/cpp/Pose2.h b/cpp/Pose2.h
index 05b015a94..4776f7a88 100644
--- a/cpp/Pose2.h
+++ b/cpp/Pose2.h
@@ -134,11 +134,14 @@ namespace gtsam {
   /**
    * compose this transformation onto another (first p1 and then p2)
    */
-  inline Pose2 compose(const Pose2& p0, const Pose2& p1)
-		{ return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t());}
+  inline Pose2 operator*(const Pose2& p1, const Pose2& p2) {
+    return Pose2(p1.r()*p2.r(), p1.t() + p1.r()*p2.t()); }
+  inline Pose2 compose(const Pose2& p1, const Pose2& p2) { return p1*p2; }
+  Pose2 compose(const Pose2& p1, const Pose2& p2,
+    boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2 = boost::none);
   Matrix Dcompose1(const Pose2& p1, const Pose2& p2);
   Matrix Dcompose2(const Pose2& p1, const Pose2& p2);
-  inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0);}
 
   /**
    * Return point coordinates in pose coordinate frame