From 8457ef4182595f10a6839b975687b4ed5215c67a Mon Sep 17 00:00:00 2001
From: nsrinivasan7 <natesh.rvce@gmail.com>
Date: Sun, 30 Nov 2014 15:42:50 -0500
Subject: [PATCH] make check works in this commit

---
 gtsam/geometry/Pose2.cpp           | 26 ++++++++++++++------------
 gtsam/geometry/Pose2.h             | 10 ++++------
 gtsam/geometry/Rot2.cpp            | 10 ++++++----
 gtsam/geometry/tests/testPose2.cpp |  4 ++--
 4 files changed, 26 insertions(+), 24 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index b95a81af0..dfda7b3b1 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -198,7 +198,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
 
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Point2& point,
-    OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
+    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
   Point2 d = transform_to(point, H1, H2);
   if (!H1 && !H2) return Rot2::relativeBearing(d);
   Matrix D_result_d;
@@ -209,27 +209,29 @@ Rot2 Pose2::bearing(const Point2& point,
 }
 
 /* ************************************************************************* */
-Rot2 Pose2::bearing(const Pose2& point,
-    OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
-  Rot2 result = bearing(point.t(), H1, H2);
+Rot2 Pose2::bearing(const Pose2& pose,
+    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+  Rot2 result = bearing(pose.t(), H1, H2);
   if (H2) {
-    Matrix2 H2_ = *H2 * point.r().matrix();
+    Matrix H2_ = *H2 * pose.r().matrix();
     *H2 = zeros(1, 3);
-    (*H2).block(0, 0, H2_.rows(), H2_.cols()) = H2_;
+    insertSub(*H2, H2_, 0, 0);
   }
   return result;
 }
-
 /* ************************************************************************* */
 double Pose2::range(const Point2& point,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const {
   Point2 d = point - t_;
   if (!H1 && !H2) return d.norm();
-  Matrix H;
+  Matrix12 H;
   double r = d.norm(H);
-  if (H1) *H1 = H * (Matrix(2, 3) <<
-      -r_.c(),  r_.s(),  0.0,
-      -r_.s(), -r_.c(),  0.0).finished();
+  if (H1) {
+      Matrix23 mvalue; // is this the correct name ?
+      mvalue << -r_.c(),  r_.s(),  0.0,
+              -r_.s(), -r_.c(),  0.0;
+      *H1 = H * mvalue;
+  }
   if (H2) *H2 = H;
   return r;
 }
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 43ee4de97..2a9f91508 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -226,8 +226,7 @@ public:
    * @return 2D rotation \f$ \in SO(2) \f$
    */
   Rot2 bearing(const Point2& point,
-      OptionalJacobian<2, 3> H1=boost::none,
-      OptionalJacobian<2, 2> H2=boost::none) const;
+               boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
 
   /**
    * Calculate bearing to another pose
@@ -235,8 +234,7 @@ public:
    * @return 2D rotation \f$ \in SO(2) \f$
    */
   Rot2 bearing(const Pose2& point,
-      OptionalJacobian<2, 3> H1=boost::none,
-      OptionalJacobian<2, 2> H2=boost::none) const;
+               boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
 
   /**
    * Calculate range to a landmark
@@ -244,8 +242,8 @@ public:
    * @return range (double)
    */
   double range(const Point2& point,
-      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+      OptionalJacobian<1, 3> H1=boost::none,
+      OptionalJacobian<1, 2> H2=boost::none) const;
 
   /**
    * Calculate range to another pose
diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index d3c6bf5f1..6a90dfb3d 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -99,12 +99,14 @@ Point2 Rot2::unrotate(const Point2& p,
 Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
   double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
   if(fabs(n) > 1e-5) {
-    if (H)
-      *H << -y / d2, x / d2;
+    if (H) {
+      (*H).block(0,0,1,2) << -y / d2, x / d2;
+    }
     return Rot2::fromCosSin(x / n, y / n);
   } else {
-    if (H)
-      (*H) << 0.0, 0.0;
+    if (H) {
+      (*H).block(0,0,1,2) << 0.0, 0.0;
+    }
     return Rot2();
   }
 }
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index b4de6eb7c..01111aafe 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -519,7 +519,7 @@ TEST( Pose2, bearing )
   expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
   EXPECT(assert_equal(expectedH1,actualH1));
   expectedH2 = numericalDerivative22(bearing_proxy, x2, l3);
-  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
 
   // establish bearing is indeed 45 degrees even if rotated
   Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
@@ -529,7 +529,7 @@ TEST( Pose2, bearing )
   expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
   expectedH2 = numericalDerivative22(bearing_proxy, x3, l4);
   EXPECT(assert_equal(expectedH1,actualH1));
-  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
 }
 
 /* ************************************************************************* */