normalize

gtsam/geometry/Point3.cpp

@@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3);
 
 /* ************************************************************************* */
 bool Point3::equals(const Point3 & q, double tol) const {
-  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
+  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
+      && fabs(z_ - q.z()) < tol);
 }
 
 /* ************************************************************************* */
@@ -62,25 +63,28 @@ Point3 Point3::operator/(double s) const {
 }
 
 /* ************************************************************************* */
-Point3 Point3::add(const Point3 &q,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  if (H1) *H1 = eye(3,3);
-  if (H2) *H2 = eye(3,3);
+Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+  if (H1)
+    *H1 = eye(3, 3);
+  if (H2)
+    *H2 = eye(3, 3);
   return *this + q;
 }
 
 /* ************************************************************************* */
-Point3 Point3::sub(const Point3 &q,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  if (H1) *H1 = eye(3,3);
-  if (H2) *H2 = -eye(3,3);
+Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+  if (H1)
+    *H1 = eye(3, 3);
+  if (H2)
+    *H2 = -eye(3, 3);
   return *this - q;
 }
 
 /* ************************************************************************* */
 Point3 Point3::cross(const Point3 &q) const {
-  return Point3( y_*q.z_ - z_*q.y_,
-      z_*q.x_ - x_*q.z_,
+  return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
       x_ * q.y_ - y_ * q.x_);
 }
 
@@ -94,6 +98,20 @@ double Point3::norm() const {
   return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
 }
 
+/* ************************************************************************* */
+Point3 Point3::normalize(boost::optional<Matrix&> H) const {
+  Point3 normalized = *this / norm();
+  if (H) {
+    // 3*3 Derivative
+    double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
+    double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
+    H->resize(3, 3);
+    *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
+    *H /= pow(x2 + y2 + z2, 1.5);
+  }
+  return normalized;
+}
+
 /* ************************************************************************* */
 ostream &operator<<(ostream &os, const Point3& p) {
   os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";

gtsam/geometry/Point3.h

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

gtsam/geometry/tests/testPoint3.cpp

@@ -14,9 +14,10 @@
  * @brief  Unit tests for Point3 class
  */
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
 
 using namespace gtsam;
 
@@ -44,8 +45,7 @@ TEST(Point3, Lie) {
 }
 
 /* ************************************************************************* */
-TEST( Point3, arithmetic)
-{
+TEST( Point3, arithmetic) {
   CHECK(P * 3 == 3 * P);
   CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
   CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
@@ -56,31 +56,42 @@ TEST( Point3, arithmetic)
 }
 
 /* ************************************************************************* */
-TEST( Point3, equals)
-{
+TEST( Point3, equals) {
   CHECK(P.equals(P));
   Point3 Q;
   CHECK(!P.equals(Q));
 }
 
 /* ************************************************************************* */
-TEST( Point3, dot)
-{
+TEST( Point3, dot) {
   Point3 origin, ones(1, 1, 1);
   CHECK(origin.dot(Point3(1, 1, 0)) == 0);
   CHECK(ones.dot(Point3(1, 1, 0)) == 2);
 }
 
 /* ************************************************************************* */
-TEST( Point3, stream)
-{
+TEST( Point3, stream) {
   Point3 p(1, 2, -3);
   std::ostringstream os;
   os << p;
   EXPECT(os.str() == "[1, 2, -3]';");
 }
 
+//*************************************************************************
+TEST (Point3, normalize) {
+  Matrix actualH;
+  Point3 point(1, -2, 3); // arbitrary point
+  Point3 expected(point / sqrt(14));
+  EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<Point3, Point3>(
+      boost::bind(&Point3::normalize, _1, boost::none), point);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */