Merge remote-tracking branch 'origin/fix/Unit3Serialization' into feature/sam_sfm_directories

gtsam/geometry/Unit3.cpp

@@ -38,6 +38,7 @@
 
 #include <boost/random/variate_generator.hpp>
 #include <iostream>
+#include <limits>
 
 using namespace std;
 
@@ -79,8 +80,7 @@ const Matrix32& Unit3::basis() const {
 #endif
 
   // Return cached version if exists
-  if (B_)
+  if (B_) return *B_;
-    return *B_;
 
   // Get the axis of rotation with the minimum projected length of the point
   Vector3 axis;
@@ -136,37 +136,39 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
 
 /* ************************************************************************* */
 Unit3 Unit3::retract(const Vector2& v) const {
-
   // Compute the 3D xi_hat vector
   Vector3 xi_hat = basis() * v;
-  double xi_hat_norm = xi_hat.norm();
+  double theta = xi_hat.norm();
 
-  // When v is the so small and approximate as a direction
+  // Treat case of very small v differently
-  if (xi_hat_norm < 1e-8) {
+  if (theta < std::numeric_limits<double>::epsilon()) {
-    return Unit3(cos(xi_hat_norm) * p_ + xi_hat);
+    return Unit3(cos(theta) * p_ + xi_hat);
   }
 
-  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_);
-  double dot = p_.dot(y.p_);
+  // Crucial quantitity here is y = theta/sin(theta) with theta=acos(x)
-
+  // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
-  // Check for special cases
+  // We treat the special caes 1 and -1 below
-  if (std::abs(dot - 1.0) < 1e-16)
+  const double x2 = x * x;
-    return Vector2(0.0, 0.0);
+  const double z = 1 - x2;
-  else if (std::abs(dot + 1.0) < 1e-16)
+  double y;
-    return Vector2(M_PI, 0.0);
+  if (z < std::numeric_limits<double>::epsilon()) {
-  else {
+    if (x > 0)  // expand at x=1
+      y = 1.0 - (x - 1.0) / 3.0;
+    else  // cop out
+      return Vector2(M_PI, 0.0);
+  } else {
     // no special case
-    const double theta = acos(dot);
+    y = acos(x) / sqrt(z);
-    Vector3 result_hat = (theta / sin(theta)) * (y.p_ - p_ * dot);
-    return basis().transpose() * result_hat;
   }
+  return basis().transpose() * y * (other.p_ - x * p_);
 }
 /* ************************************************************************* */
 

gtsam/geometry/Unit3.h

@@ -147,7 +147,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
@@ -167,13 +167,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));
   }
 
   /// @}

gtsam/geometry/tests/testUnit3.cpp

@@ -22,11 +22,13 @@
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/serializationTestHelpers.h>
+
 #include <CppUnitLite/TestHarness.h>
+
 #include <boost/bind.hpp>
 #include <boost/foreach.hpp>
 #include <boost/random.hpp>
-//#include <boost/thread.hpp>
 #include <boost/assign/std/vector.hpp>
 #include <cmath>
 
@@ -156,20 +158,39 @@ TEST(Unit3, distance) {
 
 TEST(Unit3, localCoordinates) {
   {
-    Unit3 p;
+    Unit3 p, q;
-    Vector2 actual = p.localCoordinates(p);
+    Vector2 expected = Vector2::Zero();
+    Vector2 actual = p.localCoordinates(q);
     EXPECT(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
   }
   {
     Unit3 p, q(1, 6.12385e-21, 0);
+    Vector2 expected = Vector2::Zero();
     Vector2 actual = p.localCoordinates(q);
-    CHECK(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
   }
   {
     Unit3 p, q(-1, 0, 0);
     Vector2 expected(M_PI, 0);
     Vector2 actual = p.localCoordinates(q);
-    CHECK(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+  }
+  {
+    Unit3 p, q(0, 1, 0);
+    Vector2 expected(0,-M_PI_2);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+  }
+  {
+    Unit3 p, q(0, -1, 0);
+    Vector2 expected(0, M_PI_2);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
   }
 
   double twist = 1e-4;
@@ -225,124 +246,6 @@ TEST(Unit3, retract_expmap) {
   EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
 }
 
-//*******************************************************************************
-/// Returns a random vector
-inline static Vector randomVector(const Vector& minLimits,
-    const Vector& maxLimits) {
-
-  // Get the number of dimensions and create the return vector
-  size_t numDims = dim(minLimits);
-  Vector vector = zero(numDims);
-
-  // Create the random vector
-  for (size_t i = 0; i < numDims; i++) {
-    double range = maxLimits(i) - minLimits(i);
-    vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
-  }
-  return vector;
-}
-
-//*******************************************************************************
-// Let x and y be two Unit3's.
-// The equality x.localCoordinates(x.retract(v)) == v should hold.
-TEST(Unit3, localCoordinates_retract) {
-
-  size_t numIterations = 10000;
-  Vector3 minSphereLimit(-1.0, -1.0, -1.0);
-  Vector3 maxSphereLimit(1.0, 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).
-//    boost::this_thread::sleep(boost::posix_time::milliseconds(0));
-
-    // Create the two Unit3s.
-    // NOTE: You can not create two totally random Unit3's because you cannot always compute
-    // between two any Unit3's. (For instance, they might be at the different sides of the circle).
-    Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
-//      Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
-    Vector v12 = randomVector(minXiLimit, maxXiLimit);
-    Unit3 s2 = s1.retract(v12);
-
-    // Check if the local coordinates and retract return the same results.
-    Vector actual_v12 = s1.localCoordinates(s2);
-    EXPECT(assert_equal(v12, actual_v12, 1e-8));
-    Unit3 actual_s2 = s1.retract(actual_v12);
-    EXPECT(assert_equal(s2, actual_s2, 1e-8));
-  }
-}
-
-//*******************************************************************************
-// Let x and y be two Unit3's.
-// The equality x.localCoordinates(x.retract(v)) == v should hold.
-TEST(Unit3, localCoordinates_retract_expmap) {
-
-  size_t numIterations = 10000;
-  Vector3 minSphereLimit = Vector3(-1.0, -1.0, -1.0);
-  Vector3 maxSphereLimit = Vector3(1.0, 1.0, 1.0);
-  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));
-    Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
-//      Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
-    Vector v = randomVector(minXiLimit, maxXiLimit);
-
-    // Magnitude of the rotation can be at most pi
-    if (v.norm() > M_PI)
-      v = v / M_PI;
-    Unit3 s2 = s1.retract(v);
-
-    EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6));
-    EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6));
-  }
-}
-
-//*******************************************************************************
-//TEST( Pose2, between )
-//{
-//  // <
-//  //
-//  //       ^
-//  //
-//  // *--0--*--*
-//  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
-//  Pose2 gT2(M_PI, Point2(-1,4));  // robot at (-1,4) loooking at negative x
-//
-//  Matrix actualH1,actualH2;
-//  Pose2 expected(M_PI/2.0, Point2(2,2));
-//  Pose2 actual1 = gT1.between(gT2);
-//  Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
-//  EXPECT(assert_equal(expected,actual1));
-//  EXPECT(assert_equal(expected,actual2));
-//
-//  Matrix expectedH1 = (Matrix(3,3) <<
-//      0.0,-1.0,-2.0,
-//      1.0, 0.0,-2.0,
-//      0.0, 0.0,-1.0
-//  );
-//  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
-//  EXPECT(assert_equal(expectedH1,actualH1));
-//  EXPECT(assert_equal(numericalH1,actualH1));
-//  // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
-//  EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
-//
-//  Matrix expectedH2 = (Matrix(3,3) <<
-//       1.0, 0.0, 0.0,
-//       0.0, 1.0, 0.0,
-//       0.0, 0.0, 1.0
-//  );
-//  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
-//  EXPECT(assert_equal(expectedH2,actualH2));
-//  EXPECT(assert_equal(numericalH2,actualH2));
-//
-//}
-
 //*******************************************************************************
 TEST(Unit3, Random) {
   boost::mt19937 rng(42);
@@ -354,6 +257,26 @@ TEST(Unit3, Random) {
   EXPECT(assert_equal(expectedMean,actualMean,0.1));
 }
 
+//*******************************************************************************
+// New test that uses Unit3::Random
+TEST(Unit3, localCoordinates_retract) {
+  boost::mt19937 rng(42);
+  size_t numIterations = 10000;
+
+  for (size_t i = 0; i < numIterations; i++) {
+    // Create two random Unit3s
+    const Unit3 s1 = Unit3::Random(rng);
+    const Unit3 s2 = Unit3::Random(rng);
+    // Check that they are not at opposite ends of the sphere, which is ill defined
+    if (s1.unitVector().dot(s2.unitVector())<-0.9) continue;
+
+    // Check if the local coordinates and retract return consistent results.
+    Vector v12 = s1.localCoordinates(s2);
+    Unit3 actual_s2 = s1.retract(v12);
+    EXPECT(assert_equal(s2, actual_s2, 1e-9));
+  }
+}
+
 //*************************************************************************
 TEST (Unit3, FromPoint3) {
   Matrix actualH;
@@ -365,6 +288,14 @@ TEST (Unit3, FromPoint3) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
+/* ************************************************************************* */
+TEST(actualH, Serialization) {
+  Unit3 p(0, 1, 0);
+  EXPECT(serializationTestHelpers::equalsObj(p));
+  EXPECT(serializationTestHelpers::equalsXML(p));
+  EXPECT(serializationTestHelpers::equalsBinary(p));
+}
+
 /* ************************************************************************* */
 int main() {
   srand(time(NULL));