Added derivatives to distance

gtsam/geometry/Point2.cpp

@@ -37,7 +37,23 @@ bool Point2::equals(const Point2& q, double tol) const {
 
 /* ************************************************************************* */
 double Point2::norm() const {
-  return sqrt(x_*x_ + y_*y_);
+  return sqrt(x_ * x_ + y_ * y_);
+}
+
+/* ************************************************************************* */
+static const Matrix I2 = eye(2);
+double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+  Point2 d = point - *this;
+  double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2);
+  Matrix D_result_d;
+  if (std::abs(r) > 1e-10)
+    D_result_d = Matrix_(1, 2, x / r, y / r);
+  else
+    D_result_d = Matrix_(1, 2, 1.0, 1.0);
+  if (H1) *H1 = -D_result_d;
+  if (H2) *H2 = D_result_d;
+  return r;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Point2.h

@@ -151,9 +151,8 @@ public:
   Point2 unit() const { return *this/norm(); }
 
   /** distance between two points */
-  inline double distance(const Point2& p2) const {
-    return (p2 - *this).norm();
-  }
+  double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const;
 
   /** @deprecated The following function has been deprecated, use distance above */
   inline double dist(const Point2& p2) const {

gtsam/geometry/tests/testPoint2.cpp

@@ -15,9 +15,12 @@
  * @author Frank Dellaert
  **/
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
 #include <gtsam/geometry/Point2.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/lieProxies.h>
+
+#include <CppUnitLite/TestHarness.h>
 
 using namespace std;
 using namespace gtsam;
@@ -97,6 +100,43 @@ TEST( Point2, stream)
   EXPECT(os.str() == "(1, 2)");
 }
 
+/* ************************************************************************* */
+LieVector distance_proxy(const Point2& location, const Point2& point) {
+  return LieVector(location.distance(point));
+}
+TEST( Point2, distance )
+{
+  Point2 x1, x2(1, 1), x3(1, 1);
+  Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
+  Matrix expectedH1, actualH1, expectedH2, actualH2;
+
+  // establish distance is indeed zero
+  EXPECT_DOUBLES_EQUAL(1,x1.distance(l1),1e-9);
+
+  // establish distance is indeed 45 degrees
+  EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.distance(l2),1e-9);
+
+  // Another pair
+  double actual23 = x2.distance(l3, actualH1, actualH2);
+  EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
+
+  // Check numerical derivatives
+  expectedH1 = numericalDerivative21(distance_proxy, x2, l3);
+  expectedH2 = numericalDerivative22(distance_proxy, x2, l3);
+  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
+
+  // Another test
+  double actual34 = x3.distance(l4, actualH1, actualH2);
+  EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
+
+  // Check numerical derivatives
+  expectedH1 = numericalDerivative21(distance_proxy, x3, l4);
+  expectedH2 = numericalDerivative22(distance_proxy, x3, l4);
+  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */