diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 8dcf14d4b..98b1dd4f6 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -81,6 +81,17 @@ Sphere2 Rot3::rotate(const Sphere2& p, return q; } +/* ************************************************************************* */ +Sphere2 Rot3::unrotate(const Sphere2& p, + boost::optional HR, boost::optional 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); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ea59fab17..d302a3502 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -331,6 +331,10 @@ namespace gtsam { Sphere2 rotate(const Sphere2& p, boost::optional HR = boost::none, boost::optional Hp = boost::none) const; + /// unrotate 3D direction from world frame to rotated coordinate frame + Sphere2 unrotate(const Sphere2& p, boost::optional HR = boost::none, + boost::optional Hp = boost::none) const; + /// rotate 3D direction from rotated coordinate frame to world frame Sphere2 operator*(const Sphere2& p) const; diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index b6cae287c..93a00eb2e 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -98,7 +98,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional 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; + + if (mode == Sphere2::EXPMAP) { + double xi_hat_norm = xi_hat.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(); - // 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)); + 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, - // 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."); + if (mode == Sphere2::EXPMAP) { + Matrix B = basis(); + + Vector p = Point3::Logmap(p_); + Vector q = Point3::Logmap(y.p_); + double theta = acos(p.transpose() * q); - // 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; + // the below will be nan if theta == 0.0 + if (theta == 0.0) + return (Vector (2) << 0, 0); + + Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta)); + Vector result = B.transpose() * result_hat; + + return result; + } else if (mode == Sphere2::RENORM) { + // 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); + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 0c549ed0b..18f3a789d 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -22,6 +22,10 @@ #include #include +#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; /// @} }; diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index c6e519a44..b64a00f62 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/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)), //