From 8b37da54c9bee4833b421f6723247df2f3e92cce Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 7 Oct 2014 01:20:57 +0200
Subject: [PATCH] between copy/paste :-(

---
 gtsam/geometry/Pose2.cpp | 56 ++++++++++++++++++++++++++++++++++++++++
 gtsam/geometry/Pose2.h   | 15 ++++++++---
 2 files changed, 68 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 99c81b488..90c3f5f8c 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -182,6 +182,62 @@ Point2 Pose2::transform_from(const Point2& p,
   return q + t_;
 }
 
+/* ************************************************************************* */
+Pose2 Pose2::between(const Pose2& p2) const {
+  // get cosines and sines from rotation matrices
+  const Rot2& R1 = r_, R2 = p2.r();
+  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
+
+  // Assert that R1 and R2 are normalized
+  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
+
+  // Calculate delta rotation = between(R1,R2)
+  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
+  Rot2 R(Rot2::atan2(s,c)); // normalizes
+
+  // Calculate delta translation = unrotate(R1, dt);
+  Point2 dt = p2.t() - t_;
+  double x = dt.x(), y = dt.y();
+  // t = R1' * (t2-t1)
+  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
+
+  return Pose2(R,t);
+}
+
+/* ************************************************************************* */
+Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
+    boost::optional<Matrix3&> H2) const {
+  // get cosines and sines from rotation matrices
+  const Rot2& R1 = r_, R2 = p2.r();
+  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
+
+  // Assert that R1 and R2 are normalized
+  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
+
+  // Calculate delta rotation = between(R1,R2)
+  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
+  Rot2 R(Rot2::atan2(s,c)); // normalizes
+
+  // Calculate delta translation = unrotate(R1, dt);
+  Point2 dt = p2.t() - t_;
+  double x = dt.x(), y = dt.y();
+  // t = R1' * (t2-t1)
+  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
+
+  // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
+  if (H1) {
+    double dt1 = -s2 * x + c2 * y;
+    double dt2 = -c2 * x - s2 * y;
+    *H1 <<
+        -c,  -s,  dt1,
+        s,  -c,  dt2,
+        0.0, 0.0,-1.0;
+  }
+  if (H2) *H2 = I3;
+
+  return Pose2(R,t);
+}
+
 /* ************************************************************************* */
 Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
     boost::optional<Matrix&> H2) const {
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 91b131bcb..13773ddb4 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -123,10 +123,19 @@ public:
   /**
    * Return relative pose between p1 and p2, in p1 coordinate frame
    */
-  Pose2 between(const Pose2& p2,
-      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+  Pose2 between(const Pose2& p2) const;
 
+  /**
+   * Return relative pose between p1 and p2, in p1 coordinate frame
+   */
+  Pose2 between(const Pose2& p2, boost::optional<Matrix3&> H1,
+      boost::optional<Matrix3&> H2) const;
+
+  /**
+   * Return relative pose between p1 and p2, in p1 coordinate frame
+   */
+  Pose2 between(const Pose2& p2, boost::optional<Matrix&> H1,
+      boost::optional<Matrix&> H2) const;
 
   /// @}
   /// @name Manifold