From 6b037ea49262d2f8f1ddf97d0573f143584a3187 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sat, 11 Jul 2015 18:32:10 -0700
Subject: [PATCH] bearing test

---
 gtsam/geometry/Pose3.cpp           | 26 +++++++++++++-------------
 gtsam/geometry/tests/testPose3.cpp | 16 ++++++++++++++++
 2 files changed, 29 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index f79b71c32..ac08f0797 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -315,16 +315,16 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
 /* ************************************************************************* */
 double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
                     OptionalJacobian<1, 3> H2) const {
-  Matrix36 D1;
-  Matrix3 D2;
-  Point3 local = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
+  Matrix36 D_local_pose;
+  Matrix3 D_local_point;
+  Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
   if (!H1 && !H2) {
     return local.norm();
   } else {
     Matrix13 D_r_local;
     const double r = local.norm(D_r_local);
-    if (H1) *H1 = D_r_local * D1;
-    if (H2) *H2 = D_r_local * D2;
+    if (H1) *H1 = D_r_local * D_local_pose;
+    if (H2) *H2 = D_r_local * D_local_point;
     return r;
   }
 }
@@ -332,25 +332,25 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
 /* ************************************************************************* */
 double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1,
                     OptionalJacobian<1, 6> H2) const {
-  Matrix13 D2;
-  double r = range(pose.translation(), H1, H2 ? &D2 : 0);
-  if (H2) *H2 << Matrix13::Zero(), D2 * pose.rotation().matrix();
+  Matrix13 D_local_point;
+  double r = range(pose.translation(), H1, H2 ? &D_local_point : 0);
+  if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
   return r;
 }
 
 /* ************************************************************************* */
 Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
                      OptionalJacobian<2, 3> H2) const {
-  Matrix36 D1;
-  Matrix3 D2;
-  Point3 local = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
+  Matrix36 D_local_pose;
+  Matrix3 D_local_point;
+  Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
   if (!H1 && !H2) {
     return Unit3(local);
   } else {
     Matrix23 D_b_local;
     Unit3 b = Unit3::FromPoint3(local, D_b_local);
-    if (H1)* H1 = D_b_local * D1;
-    if (H2) *H2 = D_b_local * D2;
+    if (H1) *H1 = D_b_local * D_local_pose;
+    if (H2) *H2 = D_b_local * D_local_point;
     return b;
   }
 }
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index cf6ca9bfb..f6f8b7b40 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -633,6 +633,22 @@ TEST( Pose3, range_pose )
   EXPECT(assert_equal(expectedH2,actualH2));
 }
 
+/* ************************************************************************* */
+Unit3 bearing_proxy(const Pose3& pose, const Point3& point) {
+  return pose.bearing(point);
+}
+TEST( Pose3, bearing )
+{
+  Matrix expectedH1, actualH1, expectedH2, actualH2;
+  EXPECT(assert_equal(Unit3(1,0,0),x1.bearing(l1, actualH1, actualH2),1e-9));
+
+  // Check numerical derivatives
+  expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
+  expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
+  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
+}
+
 /* ************************************************************************* */
 TEST( Pose3, unicycle )
 {