From 06520a3b1d3aa197c0ab1e45dab4e33a1cf5960a Mon Sep 17 00:00:00 2001
From: Frank <frank@skyd.io>
Date: Mon, 19 Oct 2015 14:44:00 -0700
Subject: [PATCH] Some additional derivatives (for cvross) and operators

---
 gtsam/geometry/Point3.cpp | 40 +++++++++++++++++++++++++++++++++++++--
 gtsam/geometry/Point3.h   | 39 +++++++++++++++++++-------------------
 2 files changed, 57 insertions(+), 22 deletions(-)

diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index a87aeb650..bffda9fef 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -58,6 +58,22 @@ Point3 Point3::operator/(double s) const {
   return Point3(x_ / s, y_ / s, z_ / s);
 }
 
+/* ************************************************************************* */
+double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
+                        OptionalJacobian<1, 3> H2) const {
+  double d = (p2 - *this).norm();
+  if (H1) {
+    *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z();
+    *H1 = *H1 *(1. / d);
+  }
+
+  if (H2) {
+    *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z();
+    *H2 = *H2 *(1. / d);
+  }
+  return d;
+}
+
 /* ************************************************************************* */
 Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
     OptionalJacobian<3,3> H2) const {
@@ -75,13 +91,27 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
 }
 
 /* ************************************************************************* */
-Point3 Point3::cross(const Point3 &q) const {
+Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {
+  if (H_p) {
+    *H_p << skewSymmetric(-q.vector());
+  }
+  if (H_q) {
+    *H_q << skewSymmetric(vector());
+  }
+
   return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
       x_ * q.y_ - y_ * q.x_);
 }
 
 /* ************************************************************************* */
-double Point3::dot(const Point3 &q) const {
+double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const {
+  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_);
 }
 
@@ -116,6 +146,12 @@ ostream &operator<<(ostream &os, const Point3& p) {
   return os;
 }
 
+/* ************************************************************************* */
+ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) {
+  os << p.first << " <-> " << p.second;
+  return os;
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index e8cf0be7b..e19b74d1c 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -37,8 +37,8 @@ namespace gtsam {
 
   private:
 
-    double x_, y_, z_;  
-    
+    double x_, y_, z_;
+
   public:
     enum { dimension = 3 };
 
@@ -56,7 +56,7 @@ namespace gtsam {
     /// @{
 
     /// Construct from 3-element vector
-    Point3(const Vector3& v) {
+    explicit Point3(const Vector3& v) {
       x_ = v(0);
       y_ = v(1);
       z_ = v(2);
@@ -82,6 +82,11 @@ namespace gtsam {
     /// inverse
     Point3 operator - () const { return Point3(-x_,-y_,-z_);}
 
+    /// add vector on right
+    inline Point3 operator +(const Vector3& v) const {
+      return Point3(x_ + v[0], y_ + v[1], z_ + v[2]);
+    }
+
     /// add
     Point3 operator + (const Point3& q) const;
 
@@ -99,20 +104,8 @@ namespace gtsam {
     Point3 operator / (double s) const;
 
     /** distance between two points */
-    inline double distance(const Point3& p2,
-        OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const {
-      double d = (p2 - *this).norm();
-      if (H1) {
-        *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z();
-        *H1 = *H1 *(1./d);
-      }
-
-      if (H2) {
-        *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
-        *H2 << *H2 *(1./d);
-      }
-      return d;
-    }
+    double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
+                    OptionalJacobian<1, 3> H2 = boost::none) const;
 
     /** @deprecated The following function has been deprecated, use distance above */
     inline double dist(const Point3& p2) const {
@@ -126,17 +119,19 @@ namespace gtsam {
     Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
 
     /** cross product @return this x q */
-    Point3 cross(const Point3 &q) const;
+    Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
+                                  OptionalJacobian<3, 3> H_q = boost::none) const;
 
     /** dot product @return this * q*/
-    double dot(const Point3 &q) const;
+    double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
+                                OptionalJacobian<1, 3> H_q = boost::none) const;
 
     /// @}
     /// @name Standard Interface
     /// @{
 
     /// equality
-    bool   operator ==(const Point3& q) const;
+    bool operator ==(const Point3& q) const;
 
     /** return vectorized form (column-wise)*/
     Vector3 vector() const { return Vector3(x_,y_,z_); }
@@ -192,6 +187,10 @@ namespace gtsam {
     /// @}
   };
 
+// Convenience typedef
+typedef std::pair<Point3, Point3> Point3Pair;
+std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
+
 /// Syntactic sugar for multiplying coordinates by a scalar s*p
 inline Point3 operator*(double s, const Point3& p) { return p*s;}