norm derivatives

gtsam/geometry/Point2.cpp

@@ -27,8 +27,6 @@ namespace gtsam {
 /** Explicit instantiation of base class to export members */
 INSTANTIATE_LIE(Point2);
 
-static const Matrix oneOne = (Matrix(1, 2) <<  1.0, 1.0);
-
 /* ************************************************************************* */
 void Point2::print(const string& s) const {
   cout << s << *this << endl;
@@ -39,16 +37,20 @@ bool Point2::equals(const Point2& q, double tol) const {
   return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
 }
 
+/* ************************************************************************* */
+double Point2::norm() const {
+  return sqrt(x_ * x_ + y_ * y_);
+}
+
 /* ************************************************************************* */
 double Point2::norm(boost::optional<Matrix&> H) const {
-  double r = sqrt(x_ * x_ + y_ * y_);
+  double r = norm();
   if (H) {
-    Matrix D_result_d;
+    H->resize(1,2);
     if (fabs(r) > 1e-10)
-      D_result_d = (Matrix(1, 2) <<  x_ / r, y_ / r);
+      *H << x_ / r, y_ / r;
     else
-      D_result_d = oneOne; // TODO: really infinity, why 1 here??
+      *H << 1, 1;  // really infinity, why 1 ?
-    *H = D_result_d;
   }
   return r;
 }

gtsam/geometry/Point2.h

@@ -182,7 +182,10 @@ public:
   Point2 unit() const { return *this/norm(); }
 
   /** norm of point */
-  double norm(boost::optional<Matrix&> H = boost::none) const;
+  double norm() const;
+
+  /** norm of point, with derivative */
+  double norm(boost::optional<Matrix&> H) const;
 
   /** distance between two points */
   double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,

gtsam/geometry/Point3.cpp

@@ -98,6 +98,19 @@ double Point3::norm() const {
   return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
 }
 
+/* ************************************************************************* */
+double Point3::norm(boost::optional<Matrix&> H) const {
+  double r = norm();
+  if (H) {
+    H->resize(1,3);
+    if (fabs(r) > 1e-10)
+      *H << x_ / r, y_ / r, z_ / r;
+    else
+      *H << 1, 1, 1; // really infinity, why 1 ?
+  }
+  return r;
+}
+
 /* ************************************************************************* */
 Point3 Point3::normalize(boost::optional<Matrix&> H) const {
   Point3 normalized = *this / norm();

gtsam/geometry/Point3.h

@@ -176,6 +176,9 @@ namespace gtsam {
     /** Distance of the point from the origin */
     double norm() const;
 
+    /** Distance of the point from the origin, with Jacobian */
+    double norm(boost::optional<Matrix&> H) const;
+
     /** normalize, with optional Jacobian */
     Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
 

gtsam/geometry/tests/testPoint3.cpp

@@ -16,6 +16,7 @@
 
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/LieScalar.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 
@@ -88,6 +89,20 @@ TEST (Point3, normalize) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
+//*************************************************************************
+LieScalar norm_proxy(const Point3& point) {
+  return LieScalar(point.norm());
+}
+
+TEST (Point3, norm) {
+  Matrix actualH;
+  Point3 point(3,4,5); // arbitrary point
+  double expected = sqrt(50);
+  EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8);
+  Matrix expectedH = numericalDerivative11<LieScalar, Point3>(norm_proxy, point);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;