Merged in fix/unit3retract (pull request #167)

fix: correct some inappropriate floating point error in Unit3, and its test

gtsam/geometry/Unit3.cpp

@@ -15,12 +15,12 @@
  * @author Can Erdogan
  * @author Frank Dellaert
  * @author Alex Trevor
+ * @author Zhaoyang Lv
  * @brief The Unit3 class - basically a point on a unit sphere
  */
 
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
-#include <boost/random/mersenne_twister.hpp>
 #include <gtsam/config.h> // for GTSAM_USE_TBB
 
 #ifdef __clang__
@@ -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> >
+  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;
 }
 
 #ifdef GTSAM_USE_TBB
@@ -85,26 +83,24 @@ const Matrix32& Unit3::basis() const {
     return *B_;
 
   // Get the axis of rotation with the minimum projected length of the point
-  Point3 axis;
+  Vector3 axis;
   double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
   if ((mx <= my) && (mx <= mz))
-    axis = Point3(1.0, 0.0, 0.0);
+    axis = Vector3(1.0, 0.0, 0.0);
   else if ((my <= mx) && (my <= mz))
-    axis = Point3(0.0, 1.0, 0.0);
+    axis = Vector3(0.0, 1.0, 0.0);
   else if ((mz <= mx) && (mz <= my))
-    axis = Point3(0.0, 0.0, 1.0);
+    axis = Vector3(0.0, 0.0, 1.0);
   else
     assert(false);
 
   // Create the two basis vectors
-  Point3 b1 = p_.cross(axis);
+  Vector3 b1 = p_.cross(axis).normalized();
-  b1 = b1 / b1.norm();
+  Vector3 b2 = p_.cross(b1).normalized();
-  Point3 b2 = p_.cross(b1);
-  b2 = b2 / b2.norm();
 
   // Create the basis matrix
   B_.reset(Matrix32());
-  (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
+  (*B_) << b1, b2;
   return *B_;
 }
 
@@ -122,10 +118,9 @@ 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
-  Matrix23 Bt = basis().transpose();
+  Vector2 xi = basis().transpose() * q.p_;
-  Vector2 xi = Bt * q.p_.vector();
   if (H)
-    *H = Bt * q.basis();
+    *H = basis().transpose() * q.basis();
   return xi;
 }
 
@@ -142,44 +137,34 @@ 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 xi_hat_norm = xi_hat.norm();
 
-  // Avoid nan
+  // When v is the so small and approximate as a direction
-  if (xi_hat_norm == 0.0) {
+  if (xi_hat_norm < 1e-8) {
-    if (v.norm() == 0.0)
+    return Unit3(cos(xi_hat_norm) * p_ + xi_hat);
-      return Unit3(point3());
-    else
-      return Unit3(-point3());
   }
 
-  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);
-
 }
 
 /* ************************************************************************* */
 Vector2 Unit3::localCoordinates(const Unit3& y) const {
 
-  Vector3 p = p_.vector(), q = y.p_.vector();
+  double dot = p_.dot(y.p_);
-  double dot = p.dot(q);
 
   // 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);
+    const double theta = acos(dot);
-    Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
+    Vector3 result_hat = (theta / sin(theta)) * (y.p_ - p_ * dot);
     return basis().transpose() * result_hat;
   }
 }

gtsam/geometry/Unit3.h

@@ -20,11 +20,16 @@
 
 #pragma once
 
-#include <gtsam/base/Manifold.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/dllexport.h>
 
-#include <boost/random/mersenne_twister.hpp>
 #include <boost/optional.hpp>
+#include <boost/random/mersenne_twister.hpp>
+#include <boost/serialization/nvp.hpp>
+
+#include <string>
 
 namespace gtsam {
 
@@ -33,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:
@@ -52,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
@@ -83,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);
   }
   /// @}
 
@@ -101,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

gtsam/geometry/tests/testUnit3.cpp

@@ -41,6 +41,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(Unit3)
 Point3 point3_(const Unit3& p) {
   return p.point3();
 }
+
 TEST(Unit3, point3) {
   vector<Point3> ps;
   ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
@@ -66,7 +67,7 @@ TEST(Unit3, rotate) {
   Unit3 actual = R * p;
   EXPECT(assert_equal(expected, actual, 1e-8));
   Matrix actualH, expectedH;
-  // Use numerical derivatives to calculate the expected Jacobian
+
   {
     expectedH = numericalDerivative21(rotate_, R, p);
     R.rotate(p, actualH, boost::none);
@@ -90,8 +91,8 @@ TEST(Unit3, unrotate) {
   Unit3 expected = Unit3(1, 1, 0);
   Unit3 actual = R.unrotate(p);
   EXPECT(assert_equal(expected, actual, 1e-8));
+
   Matrix actualH, expectedH;
-  // Use numerical derivatives to calculate the expected Jacobian
   {
     expectedH = numericalDerivative21(unrotate_, R, p);
     R.unrotate(p, actualH, boost::none);
@@ -113,7 +114,6 @@ TEST(Unit3, error) {
   EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
 
   Matrix actual, expected;
-  // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalDerivative11<Vector2,Unit3>(
         boost::bind(&Unit3::error, &p, _1, boost::none), q);
@@ -153,31 +153,44 @@ TEST(Unit3, distance) {
 }
 
 //*******************************************************************************
-TEST(Unit3, localCoordinates0) {
+
+TEST(Unit3, localCoordinates) {
+  {
     Unit3 p;
-  Vector actual = p.localCoordinates(p);
+    Vector2 actual = p.localCoordinates(p);
     EXPECT(assert_equal(zero(2), actual, 1e-8));
-}
+  }
-
+  {
-//*******************************************************************************
-TEST(Unit3, localCoordinates1) {
     Unit3 p, q(1, 6.12385e-21, 0);
-  Vector actual = p.localCoordinates(q);
+    Vector2 actual = p.localCoordinates(q);
     CHECK(assert_equal(zero(2), actual, 1e-8));
-}
+  }
-
+  {
-//*******************************************************************************
-TEST(Unit3, localCoordinates2) {
     Unit3 p, q(-1, 0, 0);
-  Vector expected = (Vector(2) << M_PI, 0).finished();
+    Vector2 expected(M_PI, 0);
-  Vector actual = p.localCoordinates(q);
+    Vector2 actual = p.localCoordinates(q);
     CHECK(assert_equal(expected, actual, 1e-8));
+  }
+
+  double twist = 1e-4;
+  {
+    Unit3 p(0, 1, 0), q(0 - twist, -1 + twist, 0);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(actual(0) < 1e-2);
+    EXPECT(actual(1) > M_PI - 1e-2)
+  }
+  {
+    Unit3 p(0, 1, 0), q(0 + twist, -1 - twist, 0);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(actual(0) < 1e-2);
+    EXPECT(actual(1) < -M_PI + 1e-2)
+  }
 }
 
 //*******************************************************************************
 TEST(Unit3, basis) {
   Unit3 p;
-  Matrix expected(3, 2);
+  Matrix32 expected;
   expected << 0, 0, 0, -1, 1, 0;
   Matrix actual = p.basis();
   EXPECT(assert_equal(expected, actual, 1e-8));
@@ -185,20 +198,27 @@ TEST(Unit3, basis) {
 
 //*******************************************************************************
 TEST(Unit3, retract) {
+  {
     Unit3 p;
-  Vector v(2);
+    Vector2 v(0.5, 0);
-  v << 0.5, 0;
     Unit3 expected(0.877583, 0, 0.479426);
     Unit3 actual = p.retract(v);
     EXPECT(assert_equal(expected, actual, 1e-6));
     EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+  }
+  {
+    Unit3 p;
+    Vector2 v(0, 0);
+    Unit3 actual = p.retract(v);
+    EXPECT(assert_equal(p, actual, 1e-6));
+    EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+  }
 }
 
 //*******************************************************************************
 TEST(Unit3, retract_expmap) {
   Unit3 p;
-  Vector v(2);
+  Vector2 v((M_PI / 2.0), 0);
-  v << (M_PI / 2.0), 0;
   Unit3 expected(Point3(0, 0, 1));
   Unit3 actual = p.retract(v);
   EXPECT(assert_equal(expected, actual, 1e-8));
@@ -228,9 +248,11 @@ inline static Vector randomVector(const Vector& minLimits,
 TEST(Unit3, localCoordinates_retract) {
 
   size_t numIterations = 10000;
-  Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit =
+  Vector3 minSphereLimit(-1.0, -1.0, -1.0);
-      Vector3(1.0, 1.0, 1.0);
+  Vector3 maxSphereLimit(1.0, 1.0, 1.0);
-  Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0);
+  Vector2 minXiLimit(-1.0, -1.0);
+  Vector2 maxXiLimit(1.0, 1.0);
+
   for (size_t i = 0; i < numIterations; i++) {
 
     // Sleep for the random number generator (TODO?: Better create all of them first).
@@ -246,9 +268,9 @@ TEST(Unit3, localCoordinates_retract) {
 
     // Check if the local coordinates and retract return the same results.
     Vector actual_v12 = s1.localCoordinates(s2);
-    EXPECT(assert_equal(v12, actual_v12, 1e-3));
+    EXPECT(assert_equal(v12, actual_v12, 1e-8));
     Unit3 actual_s2 = s1.retract(actual_v12);
-    EXPECT(assert_equal(s2, actual_s2, 1e-3));
+    EXPECT(assert_equal(s2, actual_s2, 1e-8));
   }
 }
 
@@ -258,30 +280,26 @@ TEST(Unit3, localCoordinates_retract) {
 TEST(Unit3, localCoordinates_retract_expmap) {
 
   size_t numIterations = 10000;
-  Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit =
+  Vector3 minSphereLimit = Vector3(-1.0, -1.0, -1.0);
-      Vector3(1.0, 1.0, 1.0);
+  Vector3 maxSphereLimit = Vector3(1.0, 1.0, 1.0);
-  Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished();
+  Vector2 minXiLimit = Vector2(-M_PI, -M_PI);
+  Vector2 maxXiLimit = Vector2(M_PI, M_PI);
+
   for (size_t i = 0; i < numIterations; i++) {
 
     // Sleep for the random number generator (TODO?: Better create all of them first).
 //    boost::this_thread::sleep(boost::posix_time::milliseconds(0));
-
-    // Create the two Unit3s.
-    // Unlike the above case, we can use any two Unit3's.
     Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
 //      Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
-    Vector v12 = randomVector(minXiLimit, maxXiLimit);
+    Vector v = randomVector(minXiLimit, maxXiLimit);
 
     // Magnitude of the rotation can be at most pi
-    if (v12.norm() > M_PI)
+    if (v.norm() > M_PI)
-      v12 = v12 / M_PI;
+      v = v / M_PI;
-    Unit3 s2 = s1.retract(v12);
+    Unit3 s2 = s1.retract(v);
 
-    // Check if the local coordinates and retract return the same results.
+    EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6));
-    Vector actual_v12 = s1.localCoordinates(s2);
+    EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6));
-    EXPECT(assert_equal(v12, actual_v12, 1e-3));
-    Unit3 actual_s2 = s1.retract(actual_v12);
-    EXPECT(assert_equal(s2, actual_s2, 1e-3));
   }
 }