skew method, minor things

gtsam/geometry/Sphere2.cpp

@@ -46,7 +46,7 @@ Matrix Sphere2::getBasis() const {
   b2 = b2 / b2.norm();
 
   // Create the basis matrix
-  Matrix B(3,2);
+  Matrix B(3, 2);
   B << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
   return B;
 }
@@ -54,8 +54,12 @@ Matrix Sphere2::getBasis() const {
 /* ************************************************************************* */
 /// The print fuction
 void Sphere2::print(const std::string& s) const {
-  printf("%s(x, y, z): (%.3lf, %.3lf, %.3lf)\n", //
+  printf("(%.3lf, %.3lf, %.3lf)\n", s.c_str(), p_.x(), p_.y(), p_.z());
-      s.c_str(), p_.x(), p_.y(), p_.z());
+}
+
+/* ************************************************************************* */
+Matrix Sphere2::skew() const {
+  return skewSymmetric(p_.x(), p_.y(), p_.z());
 }
 
 /* ************************************************************************* */

gtsam/geometry/Sphere2.h

@@ -29,29 +29,21 @@ private:
 
   Point3 p_; ///< The location of the point on the unit sphere
 
-  /// Returns the axis of rotations
-  Matrix getBasis() const;
-
 public:
 
-  /// The constructors
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
   Sphere2() :
-      p_(Point3(1.0, 0.0, 0.0)) {
+      p_(1.0, 0.0, 0.0) {
   }
 
-  /// Copy constructor
+  /// Construct from point
-  Sphere2(const Sphere2& s) {
+  Sphere2(const Point3& p) :
-    p_ = s.p_ / s.p_.norm();
+      p_(p / p.norm()) {
-  }
-
-  /// Destructor
-  ~Sphere2() {
-  }
-
-  /// Field constructor
-  Sphere2(const Point3& p) {
-    p_ = p / p.norm();
   }
+  /// @}
 
   /// @name Testable
   /// @{
@@ -65,6 +57,17 @@ public:
   }
   /// @}
 
+  /// @name Other functionality
+  /// @{
+
+  /// Returns the local coordinate frame to tangent plane
+  Matrix getBasis() const;
+
+  /// Return skew-symmetric associated with 3D point on unit sphere
+  Matrix skew() const;
+
+  /// @}
+
   /// @name Manifold
   /// @{
 
@@ -85,7 +88,6 @@ public:
   Vector localCoordinates(const Sphere2& s) const;
 
   /// @}
-
 };
 
 } // namespace gtsam