Some modernization

gtsam/geometry/Sphere2.cpp

@@ -18,66 +18,13 @@
 
 #include <gtsam/geometry/Sphere2.h>
 #include <gtsam/geometry/Point2.h>
+#include <cstdio>
 
 using namespace std;
 
 namespace gtsam {
 
-Sphere2::~Sphere2() {
+/* ************************************************************************* */
-}
-
-Sphere2 Sphere2::retract(const Vector& v) const {
-
-  // Get the vector form of the point and the basis matrix
-  Vector p = Point3::Logmap(p_);
-  Vector axis;
-  Matrix B = getBasis(&axis);
-
-  // Compute the 3D ξ^ 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; same as
-  // putting it onto the unit circle
-  Vector projected = newPoint / newPoint.norm();
-
-#ifdef DEBUG_SPHERE2_RETRACT
-  cout << "retract output for Matlab visualization (copy/paste =/): \n";
-  cout << "p = [" << p.transpose() << "];\n";
-  cout << "b1 = [" << B.col(0).transpose() << "];\n";
-  cout << "b2 = [" << B.col(1).transpose() << "];\n";
-  cout << "axis = [" << axis.transpose() << "];\n";
-  cout << "xi_hat = [" << xi_hat.transpose() << "];\n";
-  cout << "newPoint = [" << newPoint.transpose() << "];\n";
-  cout << "projected = [" << projected.transpose() << "];\n";
-#endif
-
-  Sphere2 result(Point3::Expmap(projected));
-  return result;
-}
-
-Vector Sphere2::localCoordinates(const Sphere2& y) const {
-
-  // Make sure that the angle different between x and y is less than 90. Otherwise,
-  // we can project x + ξ^ from the tangent space at x to y.
-  double cosAngle = y.p_.dot(p_);
-  assert(cosAngle > 0.0 && "Can not retract from x to y in the first place.");
-
-  // Get the basis matrix
-  Matrix B = getBasis();
-
-  // 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 [ξ1,ξ2] = (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;
-}
-
 Matrix Sphere2::getBasis(Vector* axisOutput) const {
 
   // Get the axis of rotation with the minimum projected length of the point
@@ -107,4 +54,57 @@ Matrix Sphere2::getBasis(Vector* axisOutput) const {
   return basis;
 }
 
+/* ************************************************************************* */
+/// The print fuction
+void Sphere2::print(const std::string& s) const {
+  printf("%s(x, y, z): (%.3lf, %.3lf, %.3lf)\n", s.c_str(), p_.x(), p_.y(),
+      p_.z());
+}
+
+/* ************************************************************************* */
+Sphere2 Sphere2::retract(const Vector& v) const {
+
+  // If we had a 3D point, we could just add and normalize, as in Absil
+  // Point3 newPoint = p_ + z;
+
+  // Get the vector form of the point and the basis matrix
+  Vector p = Point3::Logmap(p_);
+  Vector axis;
+  Matrix B = getBasis(&axis);
+
+  // Compute the 3D ξ^ 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; same as
+  // putting it onto the unit circle
+  Vector projected = newPoint / newPoint.norm();
+
+  return Sphere2(Point3::Expmap(projected));
+}
+
+/* ************************************************************************* */
+Vector Sphere2::localCoordinates(const Sphere2& y) const {
+
+  // Make sure that the angle different between x and y is less than 90. Otherwise,
+  // we can project x + ξ^ from the tangent space at x to y.
+  double cosAngle = y.p_.dot(p_);
+  assert(cosAngle > 0.0 && "Can not retract from x to y in the first place.");
+
+  // Get the basis matrix
+  Matrix B = getBasis();
+
+  // 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 [ξ1,ξ2] = (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;
+}
+/* ************************************************************************* */
+
 }

gtsam/geometry/Sphere2.h

@@ -24,13 +24,20 @@ namespace gtsam {
 
 /// Represents a 3D point on a unit sphere. The Sphere2 with the 3D ξ^ variable and two
 /// coefficients ξ_1 and ξ_2 that scale the 3D basis vectors of the tangent space.
-struct Sphere2 {
+class Sphere2 {
 
-  gtsam::Point3 p_; ///< The location of the point on the unit sphere
+private:
+
+  Point3 p_; ///< The location of the point on the unit sphere
+
+  /// Returns the axis of rotations
+  Matrix getBasis(Vector* axisOutput = NULL) const;
+
+public:
 
   /// The constructors
   Sphere2() :
-      p_(gtsam::Point3(1.0, 0.0, 0.0)) {
+      p_(Point3(1.0, 0.0, 0.0)) {
   }
 
   /// Copy constructor
@@ -39,10 +46,11 @@ struct Sphere2 {
   }
 
   /// Destructor
-  ~Sphere2();
+  ~Sphere2() {
+  }
 
   /// Field constructor
-  Sphere2(const gtsam::Point3& p) {
+  Sphere2(const Point3& p) {
     p_ = p / p.norm();
   }
 
@@ -50,10 +58,7 @@ struct Sphere2 {
   /// @{
 
   /// The print fuction
-  void print(const std::string& s = std::string()) const {
+  void print(const std::string& s = std::string()) const;
-    printf("%s(x, y, z): (%.3lf, %.3lf, %.3lf)\n", s.c_str(), p_.x(), p_.y(),
-        p_.z());
-  }
 
   /// The equals function with tolerance
   bool equals(const Sphere2& s, double tol = 1e-9) const {
@@ -75,15 +80,13 @@ struct Sphere2 {
   }
 
   /// The retract function
-  Sphere2 retract(const gtsam::Vector& v) const;
+  Sphere2 retract(const Vector& v) const;
 
   /// The local coordinates function
-  gtsam::Vector localCoordinates(const Sphere2& s) const;
+  Vector localCoordinates(const Sphere2& s) const;
 
   /// @}
 
-  /// Returns the axis of rotations
-  gtsam::Matrix getBasis(gtsam::Vector* axisOutput = NULL) const;
 };
 
 } // namespace gtsam