diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index 26eb98051..a8c98b205 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/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", // - s.c_str(), p_.x(), p_.y(), p_.z()); + printf("(%.3lf, %.3lf, %.3lf)\n", s.c_str(), p_.x(), p_.y(), p_.z()); +} + +/* ************************************************************************* */ +Matrix Sphere2::skew() const { + return skewSymmetric(p_.x(), p_.y(), p_.z()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index d77d7c8f0..d583853a6 100644 --- a/gtsam/geometry/Sphere2.h +++ b/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 - Sphere2(const Sphere2& s) { - p_ = s.p_ / s.p_.norm(); - } - - /// Destructor - ~Sphere2() { - } - - /// Field constructor - Sphere2(const Point3& p) { - p_ = p / p.norm(); + /// Construct from point + 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