Unit tests have been added to cover both coordinate modes, and specify mode where appropriate. Sphere2 expmap/logmap now properly handle antipodal points.

gtsam/geometry/Sphere2.cpp

@@ -109,12 +109,16 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
   
   if (mode == Sphere2::EXPMAP) {
     double xi_hat_norm = xi_hat.norm();
-    Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
 
     // Avoid nan
-    if (xi_hat_norm == 0.0)
+    if ((xi_hat_norm == 0.0)) {
+      if (v.norm () == 0.0)
         return Sphere2 (point3 ());
+      else
+        return Sphere2 (-point3 ());
+    }
     
+    Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
     return Sphere2(exp_p_xi_hat);
   } else if (mode == Sphere2::RENORM) {
     // Project onto the manifold, i.e. the closest point on the circle to the new location;
@@ -140,8 +144,10 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
     double theta = acos(p.transpose() * q);
 
     // the below will be nan if theta == 0.0
-    if (theta == 0.0)
+    if (p == q)
       return (Vector (2) << 0, 0);
+    else if (p == -q)
+      return (Vector (2) << M_PI, 0);
     
     Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
     Vector result = B.transpose() * result_hat;

gtsam/geometry/Sphere2.h

@@ -102,6 +102,13 @@ public:
     return p_;
   }
 
+  /// Return unit-norm Vector
+  Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
+    if (H)
+      *H = basis();
+    return (p_.vector ());
+  }
+
   /// Signed, vector-valued error between two directions
   Vector error(const Sphere2& q,
       boost::optional<Matrix&> H = boost::none) const;

gtsam/geometry/tests/testSphere2.cpp

@@ -81,9 +81,9 @@ static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) {
 }
 
 TEST(Sphere2, unrotate) {
-  Rot3 R = Rot3::yaw(-M_PI/2.0);
+  Rot3 R = Rot3::yaw(-M_PI/4.0);
   Sphere2 p(1, 0, 0);
-  Sphere2 expected = Sphere2(0, 1, 0);
+  Sphere2 expected = Sphere2(1, 1, 0);
   Sphere2 actual = R.unrotate (p);
   EXPECT(assert_equal(expected, actual, 1e-8));
   Matrix actualH, expectedH;
@@ -102,8 +102,8 @@ TEST(Sphere2, unrotate) {
 
 //*******************************************************************************
 TEST(Sphere2, error) {
-  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
-  r = p.retract((Vector(2) << 0.8, 0));
+  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
+  r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
   EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
   EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5));
   EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5));
@@ -126,8 +126,8 @@ TEST(Sphere2, error) {
 
 //*******************************************************************************
 TEST(Sphere2, distance) {
-  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
-  r = p.retract((Vector(2) << 0.8, 0));
+  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
+  r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
   EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
   EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8);
   EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8);
@@ -171,9 +171,20 @@ TEST(Sphere2, retract) {
   Vector v(2);
   v << 0.5, 0;
   Sphere2 expected(Point3(1, 0, 0.5));
-  Sphere2 actual = p.retract(v);
+  Sphere2 actual = p.retract(v, Sphere2::RENORM);
   EXPECT(assert_equal(expected, actual, 1e-8));
-  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+  EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8));
+}
+
+//*******************************************************************************
+TEST(Sphere2, retract_expmap) {
+  Sphere2 p;
+  Vector v(2);
+  v << (M_PI/2.0), 0;
+  Sphere2 expected(Point3(0, 0, 1));
+  Sphere2 actual = p.retract(v, Sphere2::EXPMAP);
+  EXPECT(assert_equal(expected, actual, 1e-8));
+  EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8));
 }
 
 //*******************************************************************************
@@ -223,6 +234,39 @@ TEST(Sphere2, localCoordinates_retract) {
   }
 }
 
+//*******************************************************************************
+// Let x and y be two Sphere2's.
+// The equality x.localCoordinates(x.retract(v)) == v should hold.
+TEST(Sphere2, localCoordinates_retract_expmap) {
+  
+  size_t numIterations = 10000;
+  Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
+      Vector_(3, 1.0, 1.0, 1.0);
+  Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, 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).
+    sleep(0);
+
+    // Create the two Sphere2s.
+    // Unlike the above case, we can use any two sphers.
+    Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
+//      Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
+    Vector v12 = randomVector(minXiLimit, maxXiLimit);
+    
+    // Magnitude of the rotation can be at most pi
+    if (v12.norm () > M_PI)
+      v12 = v12 / M_PI;
+    Sphere2 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-3));
+    Sphere2 actual_s2 = s1.retract(actual_v12);
+    EXPECT(assert_equal(s2, actual_s2, 1e-3));
+  }
+}
+
 //*******************************************************************************
 //TEST( Pose2, between )
 //{