support optional Jacobians for Point3::distance

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 {
+    inline double distance(const Point3& p2,
-      return (p2 - *this).norm();
+        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 */

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;