From e133e8c2e869f691bfc37512fde753d4d90625af Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Fri, 26 Sep 2014 17:20:13 -0400
Subject: [PATCH] support optional Jacobians for Point3::distance

---
 gtsam/geometry/Point3.h             | 13 +++++++++++--
 gtsam/geometry/tests/testPoint3.cpp | 17 +++++++++++++++++
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 66924ec07..96add6c32 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -164,8 +164,17 @@ namespace gtsam {
     Point3 operator / (double s) const;
 
     /** distance between two points */
-    inline double distance(const Point3& p2) const {
-      return (p2 - *this).norm();
+    inline double distance(const Point3& p2,
+        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      double d = (p2 - *this).norm();
+      if (H1) {
+        *H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z())*(1./d);
+      }
+
+      if (H2) {
+        *H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z())*(1./d);
+      }
+      return d;
     }
 
     /** @deprecated The following function has been deprecated, use distance above */
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index 65c610c25..836487c1f 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -88,6 +88,23 @@ TEST (Point3, normalize) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
+/* ************************************************************************* */
+double testFunc(const Point3& P, const Point3& Q) {
+  return P.distance(Q);
+}
+
+TEST (Point3, distance) {
+  Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
+  Matrix H1, H2;
+  double d = P.distance(Q, H1, H2);
+  double expectedDistance = 55.542686;
+  Matrix numH1 = numericalDerivative21(testFunc, P, Q);
+  Matrix numH2 = numericalDerivative22(testFunc, P, Q);
+  DOUBLES_EQUAL(expectedDistance, d, 1e-5);
+  EXPECT(assert_equal(numH1, H1, 1e-8));
+  EXPECT(assert_equal(numH2, H2, 1e-8));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;