Swicth back to Vector3 (overzealous merge).

gtsam/geometry/Unit3.cpp

@@ -15,6 +15,7 @@
  * @author Can Erdogan
  * @author Frank Dellaert
  * @author Alex Trevor
+ * @author Zhaoyang Lv
  * @brief The Unit3 class - basically a point on a unit sphere
  */
 
@@ -36,6 +37,7 @@
 
 #include <boost/random/variate_generator.hpp>
 #include <iostream>
+#include <limits>
 
 using namespace std;
 
@@ -43,12 +45,14 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
-  // 3*3 Derivative of representation with respect to point is 3*3:
+  Unit3 direction(point);
-  Matrix3 D_p_point;
+  if (H) {
-  Unit3 direction;
+    // 3*3 Derivative of representation with respect to point is 3*3:
-  direction.p_ = point.normalize(H ? &D_p_point : 0);
+    Matrix3 D_p_point;
-  if (H)
+    point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
+    // Calculate the 2*3 Jacobian
     *H << direction.basis().transpose() * D_p_point;
+  }
   return direction;
 }
 
@@ -58,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> >
+  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
-      generator(rng, randomDirection);
+      rng, randomDirection);
   vector<double> d = generator();
-  Unit3 result;
+  return Unit3(d[0], d[1], d[2]);
-  result.p_ = Point3(d[0], d[1], d[2]);
-  return result;
 }
 
 /* ************************************************************************* */
@@ -88,20 +90,19 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
 
   // Get the unit vector and derivative wrt this.
   // NOTE(hayk): We can't call point3(), because it would recursively call basis().
-  const Point3& n = p_;
+  Point3 n(p_);
 
   // Get the axis of rotation with the minimum projected length of the point
   Point3 axis;
-  double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
+  double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
-  if ((mx <= my) && (mx <= mz)) {
+  if ((mx <= my) && (mx <= mz))
     axis = Point3(1.0, 0.0, 0.0);
-  } else if ((my <= mx) && (my <= mz)) {
+  else if ((my <= mx) && (my <= mz))
     axis = Point3(0.0, 1.0, 0.0);
-  } else if ((mz <= mx) && (mz <= my)) {
+  else if ((mz <= mx) && (mz <= my))
     axis = Point3(0.0, 0.0, 1.0);
-  } else {
+  else
     assert(false);
-  }
 
   // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
   // the chosen axis.
@@ -137,17 +138,17 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
 }
 
 /* ************************************************************************* */
-const Point3& Unit3::point3(OptionalJacobian<3, 2> H) const {
+Point3 Unit3::point3(OptionalJacobian<3, 2> H) const {
   if (H)
     *H = basis();
-  return p_;
+  return Point3(p_);
 }
 
 /* ************************************************************************* */
-Vector3 Unit3::unitVector(boost::optional<Matrix&> H) const {
+Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const {
   if (H)
     *H = basis();
-  return (p_.vector());
+  return (p_);
 }
 
 /* ************************************************************************* */
@@ -194,7 +195,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,
 Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
   // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
   Matrix23 Bt = basis().transpose();
-  Vector2 xi = Bt * q.p_.vector();
+  Vector2 xi = Bt * q.p_;
   if (H_q) {
     *H_q = Bt * q.basis();
   }
@@ -248,46 +249,39 @@ 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();
-  Matrix32 B = basis();
-
   // Compute the 3D xi_hat vector
-  Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
+  Vector3 xi_hat = basis() * v;
+  double theta = xi_hat.norm();
 
-  double xi_hat_norm = xi_hat.norm();
+  // Treat case of very small v differently
-
+  if (theta < std::numeric_limits<double>::epsilon()) {
-  // Avoid nan
+    return Unit3(cos(theta) * p_ + xi_hat);
-  if (xi_hat_norm == 0.0) {
-    if (v.norm() == 0.0)
-      return Unit3(point3());
-    else
-      return Unit3(-point3());
   }
 
-  Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
+  Vector3 exp_p_xi_hat =
-      + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
+      cos(theta) * p_ + xi_hat * (sin(theta) / theta);
   return Unit3(exp_p_xi_hat);
 }
 
 /* ************************************************************************* */
-Vector2 Unit3::localCoordinates(const Unit3& y) const {
+Vector2 Unit3::localCoordinates(const Unit3& other) const {
-
+  const double x = p_.dot(other.p_);
-  Vector3 p = p_.vector(), q = y.p_.vector();
+  // Crucial quantity here is y = theta/sin(theta) with theta=acos(x)
-  double dot = p.dot(q);
+  // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
-
+  // We treat the special case 1 and -1 below
-  // Check for special cases
+  const double x2 = x * x;
-  if (dot > 1.0 - 1e-16)
+  const double z = 1 - x2;
-    return Vector2(0, 0);
+  double y;
-  else if (dot < -1.0 + 1e-16)
+  if (z < std::numeric_limits<double>::epsilon()) {
-    return Vector2(M_PI, 0);
+    if (x > 0)  // first order expansion at x=1
-  else {
+      y = 1.0 - (x - 1.0) / 3.0;
+    else  // cop out
+      return Vector2(M_PI, 0.0);
+  } else {
     // no special case
-    double theta = acos(dot);
+    y = acos(x) / sqrt(z);
-    Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
-    return basis().transpose() * result_hat;
   }
+  return basis().transpose() * y * (other.p_ - x * p_);
 }
 /* ************************************************************************* */
 

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
   mutable boost::optional<Matrix62> H_B_; ///< Cached basis derivative
 
@@ -62,23 +62,18 @@ public:
 
   /// Construct from point
   explicit Unit3(const Point3& p) :
-      p_(p.normalize()) {
+      p_(p.vector().normalized()) {
   }
 
   /// Construct from a vector3
-  explicit Unit3(const Vector3& v) :
+  explicit Unit3(const Vector3& p) :
-      p_(v / v.norm()) {
+      p_(p.normalized()) {
   }
 
   /// Construct from x,y,z
   Unit3(double x, double y, double z) :
-      p_(Point3(x, y, z).normalize()) {
+      p_(x, y, z) {
-  }
+    p_.normalize();
-
-  /// Construct from 2D point in plane at focal length f
-  /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
-  explicit Unit3(const Point2& p, double f=1.0) :
-      p_(Point3(p.x(), p.y(), f).normalize()) {
   }
 
   /// Named constructor from Point3 with optional Jacobian
@@ -100,7 +95,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);
   }
   /// @}
 
@@ -119,14 +114,14 @@ 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;
 
   /// Return unit-norm Vector
-  Vector3 unitVector(boost::optional<Matrix&> H = boost::none) const;
+  Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
 
   /// Return scaled direction as Point3
   friend Point3 operator*(double s, const Unit3& d) {
-    return s * d.p_;
+    return Point3(s * d.p_);
   }
 
   /// Return dot product with q
@@ -152,7 +147,7 @@ public:
 
   /// Cross-product w Point3
   Point3 cross(const Point3& q) const {
-    return Point3(p_.vector().cross(q.vector()));
+    return point3().cross(q);
   }
 
   /// @}
@@ -172,7 +167,7 @@ public:
 
   enum CoordinatesMode {
     EXPMAP, ///< Use the exponential map to retract
-    RENORM ///< Retract with vector addtion and renormalize.
+    RENORM ///< Retract with vector addition and renormalize.
   };
 
   /// The retract function
@@ -192,13 +187,6 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_NVP(p_);
-    // homebrew serialize Eigen Matrix
-    ar & boost::serialization::make_nvp("B11", (*B_)(0, 0));
-    ar & boost::serialization::make_nvp("B12", (*B_)(0, 1));
-    ar & boost::serialization::make_nvp("B21", (*B_)(1, 0));
-    ar & boost::serialization::make_nvp("B22", (*B_)(1, 1));
-    ar & boost::serialization::make_nvp("B31", (*B_)(2, 0));
-    ar & boost::serialization::make_nvp("B32", (*B_)(2, 1));
   }
 
   /// @}