diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index 35bd71342..49c1f3eae 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -18,66 +18,13 @@ #include #include +#include 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; +} +/* ************************************************************************* */ + } diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 45464e959..0e6b4f45f 100644 --- a/gtsam/geometry/Sphere2.h +++ b/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 { - printf("%s(x, y, z): (%.3lf, %.3lf, %.3lf)\n", s.c_str(), p_.x(), p_.y(), - p_.z()); - } + void print(const std::string& s = std::string()) const; /// 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