From 51983c30a66b68e951293978028069f7e40ca2f7 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 3 Jul 2015 11:46:51 -0700
Subject: [PATCH] Switched to Vector3 altogether

---
 gtsam/geometry/Unit3.cpp | 32 +++++++++++++-------------------
 gtsam/geometry/Unit3.h   | 28 ++++++++++++++--------------
 2 files changed, 27 insertions(+), 33 deletions(-)

diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index 82f6af9c1..a4153643e 100644
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -62,12 +62,10 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
   boost::uniform_on_sphere<double> randomDirection(3);
   // This variate_generator object is required for versions of boost somewhere
   // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
-  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> >
-      generator(rng, randomDirection);
+  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
+      rng, randomDirection);
   vector<double> d = generator();
-  Unit3 result;
-  result.p_ = Point3(d[0], d[1], d[2]);
-  return result;
+  return Unit3(d[0], d[1], d[2]);
 }
 
 #ifdef GTSAM_USE_TBB
@@ -97,8 +95,8 @@ const Matrix32& Unit3::basis() const {
     assert(false);
 
   // Create the two basis vectors
-  Vector3 b1 = p_.vector().cross(axis).normalized();
-  Vector3 b2 = p_.vector().cross(b1).normalized();
+  Vector3 b1 = p_.cross(axis).normalized();
+  Vector3 b2 = p_.cross(b1).normalized();
 
   // Create the basis matrix
   B_.reset(Matrix32());
@@ -120,7 +118,7 @@ Matrix3 Unit3::skew() const {
 /* ************************************************************************* */
 Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
   // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
-  Vector2 xi = basis().transpose() * q.p_.vector();
+  Vector2 xi = basis().transpose() * q.p_;
   if (H)
     *H = basis().transpose() * q.basis();
   return xi;
@@ -139,19 +137,16 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
 /* ************************************************************************* */
 Unit3 Unit3::retract(const Vector2& v) const {
 
-  // Get the vector form of the point and the basis matrix
-  Vector3 p = p_.vector();
-
   // Compute the 3D xi_hat vector
   Vector3 xi_hat = basis() * v;
   double xi_hat_norm = xi_hat.norm();
 
   // When v is the so small and approximate as a direction
   if (xi_hat_norm < 1e-8) {
-    return Unit3(cos(xi_hat_norm) * p + xi_hat);
+    return Unit3(cos(xi_hat_norm) * p_ + xi_hat);
   }
 
-  Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
+  Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p_
       + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
   return Unit3(exp_p_xi_hat);
 }
@@ -159,18 +154,17 @@ Unit3 Unit3::retract(const Vector2& v) const {
 /* ************************************************************************* */
 Vector2 Unit3::localCoordinates(const Unit3& y) const {
 
-  Vector3 p = p_.vector(), q = y.unitVector();
-  double dot = p.dot(q);
+  double dot = p_.dot(y.p_);
 
   // Check for special cases
   if (std::abs(dot - 1.0) < 1e-16)
-    return Vector2(0, 0);
+    return Vector2(0.0, 0.0);
   else if (std::abs(dot + 1.0) < 1e-16)
-    return Vector2(M_PI, 0);
+    return Vector2(M_PI, 0.0);
   else {
     // no special case
-    double theta = acos(dot);
-    Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
+    const double theta = acos(dot);
+    Vector3 result_hat = (theta / sin(theta)) * (y.p_ - p_ * dot);
     return basis().transpose() * result_hat;
   }
 }
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index e20b77739..9d0444844 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -38,7 +38,7 @@ class GTSAM_EXPORT Unit3 {
 
 private:
 
-  Point3 p_; ///< The location of the point on the unit sphere
+  Vector3 p_; ///< The location of the point on the unit sphere
   mutable boost::optional<Matrix32> B_; ///< Cached basis
 
 public:
@@ -57,18 +57,18 @@ public:
 
   /// Construct from point
   explicit Unit3(const Point3& p) :
-      p_(p / p.norm()) {
+      p_(p.vector().normalized()) {
   }
 
   /// Construct from a vector3
   explicit Unit3(const Vector3& p) :
-      p_(p / p.norm()) {
+      p_(p.normalized()) {
   }
 
   /// Construct from x,y,z
   Unit3(double x, double y, double z) :
       p_(x, y, z) {
-    p_ = p_ / p_.norm();
+    p_.normalize();
   }
 
   /// Named constructor from Point3 with optional Jacobian
@@ -88,7 +88,7 @@ public:
 
   /// The equals function with tolerance
   bool equals(const Unit3& s, double tol = 1e-9) const {
-    return p_.equals(s.p_, tol);
+    return equal_with_abs_tol(p_, s.p_, tol);
   }
   /// @}
 
@@ -106,22 +106,22 @@ public:
   Matrix3 skew() const;
 
   /// Return unit-norm Point3
-  const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const {
+  Point3 point3(OptionalJacobian<3, 2> H = boost::none) const {
+    if (H)
+      *H = basis();
+    return Point3(p_);
+  }
+
+  /// Return unit-norm Vector
+  const Vector3& unitVector(boost::optional<Matrix&> H = boost::none) const {
     if (H)
       *H = basis();
     return p_;
   }
 
-  /// Return unit-norm Vector
-  Vector3 unitVector(boost::optional<Matrix&> H = boost::none) const {
-    if (H)
-      *H = basis();
-    return (p_.vector());
-  }
-
   /// Return scaled direction as Point3
   friend Point3 operator*(double s, const Unit3& d) {
-    return s * d.p_;
+    return Point3(s * d.p_);
   }
 
   /// Signed, vector-valued error between two directions