From 4d495a0267d217e1e6b66279526b06c66c3f73ba Mon Sep 17 00:00:00 2001
From: nsrinivasan7 <natesh.rvce@gmail.com>
Date: Sat, 29 Nov 2014 15:39:43 -0500
Subject: [PATCH] changed the between factor in Pose2 using Optional Jacobian.
 This fixes build errors in unstable also.

---
 gtsam/geometry/Pose2.cpp | 60 ++--------------------------------------
 gtsam/geometry/Pose2.h   | 15 ++--------
 2 files changed, 4 insertions(+), 71 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index f0daa4054..c63eb844c 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -162,30 +162,8 @@ Point2 Pose2::transform_from(const Point2& p,
 }
 
 /* ************************************************************************* */
-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 {
+Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
+    OptionalJacobian<3,3> 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();
@@ -217,40 +195,6 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
   return Pose2(R,t);
 }
 
-/* ************************************************************************* */
-Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> 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 = (Matrix(3, 3) <<
-        -c,  -s,  dt1,
-        s,  -c,  dt2,
-        0.0, 0.0,-1.0).finished();
-  }
-  if (H2) *H2 = I3;
-
-  return Pose2(R,t);
-}
-
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Point2& point,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index f39ca5a81..6caeb196a 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -122,19 +122,8 @@ public:
   /**
    * Return relative pose between p1 and p2, in p1 coordinate frame
    */
-  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;
+  Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none,
+      OptionalJacobian<3,3> H = boost::none) const;
 
   /// @}
   /// @name Manifold