From e618df7bfaa125bd0d7ad5f0f3b2d3416c9e14fc Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jun 2013 05:52:14 +0000
Subject: [PATCH] Improved range and derivatives (no more rotating of
 translation, as range is invariant to it)

---
 gtsam/geometry/Pose2.cpp | 30 +++++++++++++++++-------------
 1 file changed, 17 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 6229e4077..c0a3d43d2 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -221,27 +221,31 @@ Rot2 Pose2::bearing(const Pose2& point,
 /* ************************************************************************* */
 double Pose2::range(const Point2& point,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  if (!H1 && !H2)  return transform_to(point).norm();
-  Point2 d = transform_to(point, H1, H2);
+  Point2 d = point - t_;
+  if (!H1 && !H2) return d.norm();
   Matrix H;
   double r = d.norm(H);
-  if (H1) *H1 = H * (*H1);
-  if (H2) *H2 = H * (*H2);
+  if (H1) *H1 = H * Matrix_(2, 3,
+      -r_.c(),  r_.s(),  0.0,
+      -r_.s(), -r_.c(),  0.0);
+  if (H2) *H2 = H;
   return r;
 }
 
 /* ************************************************************************* */
-double Pose2::range(const Pose2& point,
+double Pose2::range(const Pose2& pose2,
     boost::optional<Matrix&> H1,
     boost::optional<Matrix&> H2) const {
-  double r = range(point.t(), H1, H2);
-  if (H2) {
-    // NOTE: expmap changes the orientation of expmap direction,
-    // so we must rotate the jacobian
-    Matrix H2_ = *H2 * point.r().matrix();
-    *H2 = zeros(1, 3);
-    insertSub(*H2, H2_, 0, 0);
-  }
+  Point2 d = pose2.t() - t_;
+  if (!H1 && !H2) return d.norm();
+  Matrix 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);
+  if (H2) *H2 = H * Matrix_(2, 3,
+      pose2.r_.c(), -pose2.r_.s(),  0.0,
+      pose2.r_.s(),  pose2.r_.c(),  0.0);
   return r;
 }