Added expmap coordinate mode option for Sphere2 retract and local coordinates, as well as unrotate for Sphere2.

gtsam/geometry/Rot3.cpp

@@ -81,6 +81,17 @@ Sphere2 Rot3::rotate(const Sphere2& p,
   return q;
 }
 
+/* ************************************************************************* */
+Sphere2 Rot3::unrotate(const Sphere2& p,
+    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
+  Sphere2 q = unrotate(p.point3(Hp));
+  if (Hp)
+    (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
+  if (HR)
+    (*HR) = q.basis().transpose() * q.skew();
+  return q;
+}
+
 /* ************************************************************************* */
 Sphere2 Rot3::operator*(const Sphere2& p) const {
   return rotate(p);

gtsam/geometry/Rot3.h

@@ -331,6 +331,10 @@ namespace gtsam {
     Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
         boost::optional<Matrix&> Hp = boost::none) const;
 
+    /// unrotate 3D direction from world frame to rotated coordinate frame
+    Sphere2 unrotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
+        boost::optional<Matrix&> Hp = boost::none) const;
+
     /// rotate 3D direction from rotated coordinate frame to world frame
     Sphere2 operator*(const Sphere2& p) const;
 

gtsam/geometry/Sphere2.cpp

@@ -98,7 +98,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
 }
 
 /* ************************************************************************* */
-Sphere2 Sphere2::retract(const Vector& v) const {
+Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
 
   // Get the vector form of the point and the basis matrix
   Vector p = Point3::Logmap(p_);
@@ -106,35 +106,64 @@ Sphere2 Sphere2::retract(const Vector& v) const {
 
   // Compute the 3D xi_hat vector
   Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
-  Vector newPoint = p + xi_hat;
   
-  // Project onto the manifold, i.e. the closest point on the circle to the new location;
+  if (mode == Sphere2::EXPMAP) {
-  // same as putting it onto the unit circle
+    double xi_hat_norm = xi_hat.norm();
-  Vector projected = newPoint / newPoint.norm();
+    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;
+    // same as putting it onto the unit circle
+    Vector newPoint = p + xi_hat;
+    Vector projected = newPoint / newPoint.norm();
 
-  return Sphere2(Point3::Expmap(projected));
+    return Sphere2(Point3::Expmap(projected));
+  } else {
+    assert (false);
+    exit (1);
+  }  
 }
 
 /* ************************************************************************* */
-Vector Sphere2::localCoordinates(const Sphere2& y) const {
+  Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const {
 
-  // Make sure that the angle different between x and y is less than 90. Otherwise,
+  if (mode == Sphere2::EXPMAP) {
-  // we can project x + xi_hat from the tangent space at x to y.
+    Matrix B = basis();
-  assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
     
-  // Get the basis matrix
+    Vector p = Point3::Logmap(p_);
-  Matrix B = basis();
+    Vector q = Point3::Logmap(y.p_);
+    double theta = acos(p.transpose() * q);
 
-  // Create the vector forms of p and q (the Point3 of y).
+    // the below will be nan if theta == 0.0
-  Vector p = Point3::Logmap(p_);
+    if (theta == 0.0)
-  Vector q = Point3::Logmap(y.p_);
+      return (Vector (2) << 0, 0);
     
-  // Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
+    Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
-  double alpha = p.transpose() * q;
+    Vector result = B.transpose() * result_hat;
-  assert(alpha != 0.0);
+    
-  Matrix coeffs = (B.transpose() * q) / alpha;
+    return result;
-  Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
+  } else if (mode == Sphere2::RENORM) {
-  return result;
+    // Make sure that the angle different between x and y is less than 90. Otherwise,
+    // we can project x + xi_hat from the tangent space at x to y.
+    assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
+    
+    // Get the basis matrix
+    Matrix B = basis();
+    
+    // Create the vector forms of p and q (the Point3 of y).
+    Vector p = Point3::Logmap(p_);
+    Vector q = Point3::Logmap(y.p_);
+    
+    // Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
+    double alpha = p.transpose() * q;
+    assert(alpha != 0.0);
+    Matrix coeffs = (B.transpose() * q) / alpha;
+    Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
+    return result;
+  } else {
+    assert (false);
+    exit (1);
+  }
 }
 /* ************************************************************************* */
 

gtsam/geometry/Sphere2.h

@@ -22,6 +22,10 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/DerivedValue.h>
 
+#ifndef SPHERE2_DEFAULT_COORDINATES_MODE
+  #define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::EXPMAP
+#endif
+
 // (Cumbersome) forward declaration for random generator
 namespace boost {
 namespace random {
@@ -121,11 +125,16 @@ public:
     return 2;
   }
 
+  enum CoordinatesMode {
+    EXPMAP, ///< Use the exponential map to retract
+    RENORM ///< Retract with vector addtion and renormalize.
+  };
+
   /// The retract function
-  Sphere2 retract(const Vector& v) const;
+  Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
 
   /// The local coordinates function
-  Vector localCoordinates(const Sphere2& s) const;
+  Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
 
   /// @}
 };

gtsam/geometry/tests/testSphere2.cpp

@@ -75,6 +75,31 @@ TEST(Sphere2, rotate) {
   }
 }
 
+//*******************************************************************************
+static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) {
+  return R.unrotate (p);
+}
+
+TEST(Sphere2, unrotate) {
+  Rot3 R = Rot3::yaw(-M_PI/2.0);
+  Sphere2 p(1, 0, 0);
+  Sphere2 expected = Sphere2(0, 1, 0);
+  Sphere2 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);
+    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+  }
+  {
+    expectedH = numericalDerivative22(unrotate_, R, p);
+    R.unrotate(p, boost::none, actualH);
+    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+  }
+}
+
 //*******************************************************************************
 TEST(Sphere2, error) {
   Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //