Simplified Pose2::range

gtsam/geometry/Point2.cpp

@@ -25,6 +25,8 @@ 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;
@@ -36,8 +38,17 @@ bool Point2::equals(const Point2& q, double tol) const {
 }
 
 /* ************************************************************************* */
-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_);
+  if (H) {
+    Matrix D_result_d;
+    if (std::abs(r) > 1e-10)
+      D_result_d = Matrix_(1, 2, x_ / r, y_ / r);
+    else
+      D_result_d = oneOne; // TODO: really infinity, why 1 here??
+    *H = D_result_d;
+  }
+  return r;
 }
 
 /* ************************************************************************* */
@@ -45,15 +56,14 @@ 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;
+  if (H1 || H2) {
+    Matrix H;
+    double r = d.norm(H);
+    if (H1) *H1 = -H;
+    if (H2) *H2 =  H;
+    return r;
+  } else
+    return d.norm();
 }
 
 /* ************************************************************************* */

gtsam/geometry/Point2.h

@@ -144,12 +144,12 @@ public:
   /// @name Vector Space
   /// @{
 
-  /** norm of point */
-  double norm() const;
-
   /** creates a unit vector */
   Point2 unit() const { return *this/norm(); }
 
+  /** norm of point */
+  double norm(boost::optional<Matrix&> H = boost::none) const;
+
   /** distance between two points */
   double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none) const;

gtsam/geometry/Pose2.cpp

@@ -223,15 +223,10 @@ 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);
-  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 * (*H1);
-  if (H2) *H2 = D_result_d * (*H2);
+  Matrix H;
+  double r = d.norm(H);
+  if (H1) *H1 = H * (*H1);
+  if (H2) *H2 = H * (*H2);
   return r;
 }
 

gtsam/geometry/tests/testPoint2.cpp

@@ -72,16 +72,6 @@ TEST( Point2, arithmetic)
   EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2));
 }
 
-/* ************************************************************************* */
-TEST( Point2, norm)
-{
-  Point2 p0(cos(5.0), sin(5.0));
-  DOUBLES_EQUAL(1,p0.norm(),1e-6);
-  Point2 p1(4, 5), p2(1, 1);
-  DOUBLES_EQUAL( 5,p1.distance(p2),1e-6);
-  DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6);
-}
-
 /* ************************************************************************* */
 TEST( Point2, unit)
 {
@@ -92,12 +82,35 @@ TEST( Point2, unit)
 }
 
 /* ************************************************************************* */
-TEST( Point2, stream)
+// some shared test values
+Point2 x1, x2(1, 1), x3(1, 1);
+Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
+
+/* ************************************************************************* */
+LieVector norm_proxy(const Point2& point) {
+  return LieVector(point.norm());
+}
+TEST( Point2, norm )
 {
-  Point2 p(1,2);
-  std::ostringstream os;
-  os << p;
-  EXPECT(os.str() == "(1, 2)");
+  Point2 p0(cos(5.0), sin(5.0));
+  DOUBLES_EQUAL(1,p0.norm(),1e-6);
+  Point2 p1(4, 5), p2(1, 1);
+  DOUBLES_EQUAL( 5,p1.distance(p2),1e-6);
+  DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6);
+
+  Matrix expectedH, actualH;
+  double actual ;
+
+  // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1]
+  actual = x1.norm(actualH);
+  EXPECT_DOUBLES_EQUAL(0,actual,1e-9);
+  expectedH = Matrix_(1, 2, 1.0, 1.0);
+  EXPECT(assert_equal(expectedH,actualH));
+
+  actual = x2.norm(actualH);
+  EXPECT_DOUBLES_EQUAL(sqrt(2),actual,1e-9);
+  expectedH = numericalDerivative11(norm_proxy, x2);
+  EXPECT(assert_equal(expectedH,actualH));
 }
 
 /* ************************************************************************* */
@@ -106,8 +119,6 @@ LieVector distance_proxy(const Point2& location, const Point2& 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
@@ -137,6 +148,15 @@ TEST( Point2, distance )
   EXPECT(assert_equal(expectedH2,actualH2));
 }
 
+/* ************************************************************************* */
+TEST( Point2, stream)
+{
+  Point2 p(1,2);
+  std::ostringstream os;
+  os << p;
+  EXPECT(os.str() == "(1, 2)");
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/geometry/tests/testPose2.cpp

@@ -14,22 +14,21 @@
  * @brief  Unit tests for Pose2 class
  */
 
-#include <cmath>
-#include <iostream>
-
-#include <boost/foreach.hpp>
-#include <boost/optional.hpp>
-#include <boost/assign/std/vector.hpp> // for operator +=
-using namespace boost::assign;
-
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/lieProxies.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Rot2.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/lieProxies.h>
 
+#include <CppUnitLite/TestHarness.h>
+#include <boost/foreach.hpp>
+#include <boost/optional.hpp>
+#include <boost/assign/std/vector.hpp> // for operator +=
+#include <cmath>
+#include <iostream>
+
+using namespace boost::assign;
 using namespace gtsam;
 using namespace std;