Made all methods with derivatives available as free functions

gtsam/geometry/Point3.cpp

@@ -33,45 +33,54 @@ void Point3::print(const string& s) const {
 }
 
 /* ************************************************************************* */
-
 bool Point3::operator==(const Point3& q) const {
   return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
 }
 
-/* ************************************************************************* */
 Point3 Point3::operator+(const Point3& q) const {
   return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
 }
 
-/* ************************************************************************* */
 Point3 Point3::operator-(const Point3& q) const {
   return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
 }
 
-/* ************************************************************************* */
 Point3 Point3::operator*(double s) const {
   return Point3(x_ * s, y_ * s, z_ * s);
 }
 
-/* ************************************************************************* */
 Point3 Point3::operator/(double s) const {
   return Point3(x_ / s, y_ / s, z_ / s);
 }
 
 /* ************************************************************************* */
-double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
+double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
                         OptionalJacobian<1, 3> H2) const {
-  double d = (p2 - *this).norm();
+  return gtsam::distance(*this,q,H1,H2);
-  if (H1) {
+}
-    *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z();
-    *H1 = *H1 *(1. / d);
-  }
 
-  if (H2) {
+double Point3::norm(OptionalJacobian<1,3> H) const {
-    *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z();
+  return gtsam::norm(*this, H);
-    *H2 = *H2 *(1. / d);
+}
-  }
+
-  return d;
+Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
+  return gtsam::normalize(*this, H);
+}
+
+Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1,
+                     OptionalJacobian<3, 3> H2) const {
+  return gtsam::cross(*this, q, H1, H2);
+}
+
+double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1,
+                   OptionalJacobian<1, 3> H2) const {
+  return gtsam::dot(*this, q, H1, H2);
+}
+
+/* ************************************************************************* */
+ostream &operator<<(ostream &os, const Point3& p) {
+  os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
+  return os;
 }
 
 /* ************************************************************************* */
@@ -92,59 +101,56 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
 #endif
 
 /* ************************************************************************* */
-Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {
+double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
-  if (H_p) {
+                OptionalJacobian<1, 3> H2) {
-    *H_p << skewSymmetric(-q.vector());
+  double d = (q - p1).norm();
+  if (H1) {
+    *H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z();
+    *H1 = *H1 *(1. / d);
   }
-  if (H_q) {
+  if (H2) {
-    *H_q << skewSymmetric(vector());
+    *H2 << -p1.x() + q.x(), -p1.y() + q.y(), -p1.z() + q.z();
+    *H2 = *H2 *(1. / d);
   }
-
+  return d;
-  return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
-      x_ * q.y_ - y_ * q.x_);
 }
 
-/* ************************************************************************* */
+double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
-double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const {
+  double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
-  if (H_p) {
-    *H_p << q.vector().transpose();
-  }
-  if (H_q) {
-    *H_q << vector().transpose();
-  }
-
-  return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
-}
-
-/* ************************************************************************* */
-double Point3::norm(OptionalJacobian<1,3> H) const {
-  double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
   if (H) {
     if (fabs(r) > 1e-10)
-      *H << x_ / r, y_ / r, z_ / r;
+      *H << p.x() / r, p.y() / r, p.z() / r;
     else
-      *H << 1, 1, 1; // really infinity, why 1 ?
+      *H << 1, 1, 1;  // really infinity, why 1 ?
   }
   return r;
 }
 
-/* ************************************************************************* */
+Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) {
-Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
+  Point3 normalized = p / norm(p);
-  Point3 normalized = *this / norm();
   if (H) {
     // 3*3 Derivative
-    double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
+    double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z();
-    double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
+    double xy = p.x() * p.y(), xz = p.x() * p.z(), yz = p.y() * p.z();
-    *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
+    *H << y2 + z2, -xy, -xz, /**/ -xy, x2 + z2, -yz, /**/ -xz, -yz, x2 + y2;
     *H /= pow(x2 + y2 + z2, 1.5);
   }
   return normalized;
 }
 
-/* ************************************************************************* */
+Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
-ostream &operator<<(ostream &os, const Point3& p) {
+             OptionalJacobian<3, 3> H2) {
-  os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
+  if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z());
-  return os;
+  if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z());
+  return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(),
+                p.x() * q.y() - p.y() * q.x());
+}
+
+double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
+           OptionalJacobian<1, 3> H2) {
+  if (H1) *H1 << q.x(), q.y(), q.z();
+  if (H2) *H2 << p.x(), p.y(), p.z();
+  return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
 }
 
 /* ************************************************************************* */

gtsam/geometry/Point3.h

@@ -104,14 +104,14 @@ namespace gtsam {
     Point3 operator / (double s) const;
 
     /** distance between two points */
-    double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
+    double distance(const Point3& q, OptionalJacobian<1, 3> H1 = boost::none,
                     OptionalJacobian<1, 3> H2 = boost::none) const;
 
     /** Distance of the point from the origin, with Jacobian */
     double norm(OptionalJacobian<1,3> H = boost::none) const;
 
     /** normalize, with optional Jacobian */
-    Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
+    Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
 
     /** cross product @return this x q */
     Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
@@ -155,7 +155,8 @@ namespace gtsam {
     Point3 retract(const Vector3& v) const { return compose(Point3(v));}
     static Vector3 Logmap(const Point3& p) { return p.vector();}
     static Point3 Expmap(const Vector3& v) { return Point3(v);}
-    inline double dist(const Point3& p2) const { return (p2 - *this).norm(); }
+    inline double dist(const Point3& q) const { return (q - *this).norm(); }
+    Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
     Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
                OptionalJacobian<3, 3> H2 = boost::none) const;
     Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
@@ -194,6 +195,31 @@ struct traits<Point3> : public internal::VectorSpace<Point3> {};
 template<>
 struct traits<const Point3> : public internal::VectorSpace<Point3> {};
 
+/// distance between two points
+double distance(const Point3& p1, const Point3& q,
+                OptionalJacobian<1, 3> H1 = boost::none,
+                OptionalJacobian<1, 3> H2 = boost::none);
+
+/// Distance of the point from the origin, with Jacobian
+double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
+
+/// normalize, with optional Jacobian
+Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
+
+/// cross product @return this x q
+Point3 cross(const Point3& p, const Point3& q,
+             OptionalJacobian<3, 3> H_p = boost::none,
+             OptionalJacobian<3, 3> H_q = boost::none);
+
+/// dot product
+double dot(const Point3& p, const Point3& q,
+           OptionalJacobian<1, 3> H_p = boost::none,
+           OptionalJacobian<1, 3> H_q = boost::none);
+
+// Convenience typedef
+typedef std::pair<Point3, Point3> Point3Pair;
+std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
+
 template <typename A1, typename A2>
 struct Range;
 
@@ -203,7 +229,7 @@ struct Range<Point3, Point3> {
   double operator()(const Point3& p, const Point3& q,
                     OptionalJacobian<1, 3> H1 = boost::none,
                     OptionalJacobian<1, 3> H2 = boost::none) {
-    return p.distance(q, H1, H2);
+    return distance(p, q, H1, H2);
   }
 };
 

gtsam/geometry/Unit3.cpp

@@ -44,7 +44,7 @@ namespace gtsam {
 Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
   // 3*3 Derivative of representation with respect to point is 3*3:
   Matrix3 D_p_point;
-  Point3 normalized = point.normalize(H ? &D_p_point : 0);
+  Point3 normalized = normalize(point, H ? &D_p_point : 0);
   Unit3 direction;
   direction.p_ = normalized.vector();
   if (H)
@@ -108,7 +108,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
 
   // Normalize result to get a unit vector: b1 = B1 / |B1|.
   Matrix33 H_b1_B1;
-  Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0);
+  Point3 b1 = normalize(B1, H ? &H_b1_B1 : 0);
 
   // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
   // No need to normalize this, p and b1 are orthogonal unit vectors.

gtsam/geometry/tests/testPoint3.cpp

@@ -96,9 +96,9 @@ TEST (Point3, normalize) {
   Matrix actualH;
   Point3 point(1, -2, 3); // arbitrary point
   Point3 expected(point / sqrt(14.0));
-  EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
+  EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<Point3, Point3>(
-      boost::bind(&Point3::normalize, _1, boost::none), point);
+      boost::bind(gtsam::normalize, _1, boost::none), point);
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 

python/handwritten/geometry/Point3.cpp

@@ -41,7 +41,7 @@ class_<Point3>("Point3")
   .def("dot", &Point3::dot)
   .def("equals", &Point3::equals, equals_overloads(args("q","tol")))
   .def("norm", &Point3::norm)
-  .def("normalize", &Point3::normalize)
+  .def("normalized", &Point3::normalized)
   .def("print", &Point3::print, print_overloads(args("s")))
   .def("vector", &Point3::vector)
   .def("x", &Point3::x)