diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index 2069fe0ee..69dd77239 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -29,8 +29,7 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Sphere2 Sphere2::FromPoint3(const Point3& point, - boost::optional H) { +Sphere2 Sphere2::FromPoint3(const Point3& point, boost::optional H) { Sphere2 direction(point); if (H) { // 3*3 Derivative of representation with respect to point is 3*3: @@ -114,7 +113,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional H) const { } /* ************************************************************************* */ -Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const { +Sphere2 Sphere2::retract(const Vector& v) const { // Get the vector form of the point and the basis matrix Vector p = Point3::Logmap(p_); @@ -122,75 +121,42 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const { // Compute the 3D xi_hat vector Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); - - if (mode == Sphere2::EXPMAP) { - double xi_hat_norm = xi_hat.norm(); - // Avoid nan - if (xi_hat_norm == 0.0) { - if (v.norm () == 0.0) - return Sphere2 (point3 ()); - else - return Sphere2 (-point3 ()); - } - - 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(); + double xi_hat_norm = xi_hat.norm(); - return Sphere2(Point3::Expmap(projected)); - } else { - assert (false); - exit (1); - } -} - -/* ************************************************************************* */ - Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const { - - if (mode == Sphere2::EXPMAP) { - Matrix B = basis(); - - Vector p = Point3::Logmap(p_); - Vector q = Point3::Logmap(y.p_); - double theta = acos(p.transpose() * q); - - // the below will be nan if theta == 0.0 - if (p == q) - return (Vector (2) << 0, 0); - else if (p == (Vector)-q) - return (Vector (2) << M_PI, 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); + // Avoid nan + if (xi_hat_norm == 0.0) { + if (v.norm() == 0.0) + return Sphere2(point3()); + else + return Sphere2(-point3()); } + + 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); + +} + +/* ************************************************************************* */ +Vector Sphere2::localCoordinates(const Sphere2& y) const { + + Matrix B = basis(); + + Vector p = Point3::Logmap(p_); + Vector q = Point3::Logmap(y.p_); + double theta = acos(p.transpose() * q); + + // the below will be nan if theta == 0.0 + if (p == q) + return (Vector(2) << 0, 0); + else if (p == (Vector) -q) + return (Vector(2) << M_PI, 0); + + Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta)); + Vector result = B.transpose() * result_hat; + + return result; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 507fc5135..1f155a08c 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -23,10 +23,6 @@ #include #include -#ifndef SPHERE2_DEFAULT_COORDINATES_MODE - #define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM -#endif - // (Cumbersome) forward declaration for random generator namespace boost { namespace random { @@ -71,8 +67,8 @@ public: } /// Named constructor from Point3 with optional Jacobian - static Sphere2 FromPoint3(const Point3& point, - boost::optional H = boost::none); + static Sphere2 FromPoint3(const Point3& point, boost::optional H = + boost::none); /// Random direction, using boost::uniform_on_sphere static Sphere2 Random(boost::random::mt19937 & rng); @@ -113,7 +109,7 @@ public: /// Return scaled direction as Point3 friend Point3 operator*(double s, const Sphere2& d) { - return s*d.p_; + return s * d.p_; } /// Signed, vector-valued error between two directions @@ -145,10 +141,10 @@ public: }; /// The retract function - Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; + Sphere2 retract(const Vector& v) const; /// The local coordinates function - Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; + Vector localCoordinates(const Sphere2& s) const; /// @} }; diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index 706aa8fb4..4fffc4b9f 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -80,14 +80,14 @@ TEST(Sphere2, rotate) { //******************************************************************************* static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) { - return R.unrotate (p); + return R.unrotate(p); } TEST(Sphere2, unrotate) { - Rot3 R = Rot3::yaw(-M_PI/4.0); + Rot3 R = Rot3::yaw(-M_PI / 4.0); Sphere2 p(1, 0, 0); Sphere2 expected = Sphere2(1, 1, 0); - Sphere2 actual = R.unrotate (p); + Sphere2 actual = R.unrotate(p); EXPECT(assert_equal(expected, actual, 1e-8)); Matrix actualH, expectedH; // Use numerical derivatives to calculate the expected Jacobian @@ -105,11 +105,11 @@ TEST(Sphere2, unrotate) { //******************************************************************************* TEST(Sphere2, error) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // - r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // + r = p.retract((Vector(2) << 0.8, 0)); EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8)); - EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5)); - EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5)); + EXPECT(assert_equal((Vector(2) << 0.479426, 0), p.error(q), 1e-5)); + EXPECT(assert_equal((Vector(2) << 0.717356, 0), p.error(r), 1e-5)); Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian @@ -129,11 +129,11 @@ TEST(Sphere2, error) { //******************************************************************************* TEST(Sphere2, distance) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // - r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // + r = p.retract((Vector(2) << 0.8, 0)); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); - EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8); - EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8); + EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8); + EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8); Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian @@ -173,21 +173,21 @@ TEST(Sphere2, retract) { Sphere2 p; Vector v(2); v << 0.5, 0; - Sphere2 expected(Point3(1, 0, 0.5)); - Sphere2 actual = p.retract(v, Sphere2::RENORM); - EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8)); + Sphere2 expected(0.877583, 0, 0.479426); + Sphere2 actual = p.retract(v); + EXPECT(assert_equal(expected, actual, 1e-6)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); } //******************************************************************************* TEST(Sphere2, retract_expmap) { Sphere2 p; Vector v(2); - v << (M_PI/2.0), 0; + v << (M_PI / 2.0), 0; Sphere2 expected(Point3(0, 0, 1)); - Sphere2 actual = p.retract(v, Sphere2::EXPMAP); + Sphere2 actual = p.retract(v); EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); } //******************************************************************************* @@ -241,11 +241,12 @@ TEST(Sphere2, localCoordinates_retract) { // Let x and y be two Sphere2's. // The equality x.localCoordinates(x.retract(v)) == v should hold. TEST(Sphere2, localCoordinates_retract_expmap) { - + size_t numIterations = 10000; Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = Vector_(3, 1.0, 1.0, 1.0); - Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI); + Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, + M_PI); for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). @@ -256,9 +257,9 @@ TEST(Sphere2, localCoordinates_retract_expmap) { Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); // Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); Vector v12 = randomVector(minXiLimit, maxXiLimit); - + // Magnitude of the rotation can be at most pi - if (v12.norm () > M_PI) + if (v12.norm() > M_PI) v12 = v12 / M_PI; Sphere2 s2 = s1.retract(v12); @@ -321,7 +322,7 @@ TEST(Sphere2, Random) { Point3 expectedMean, actualMean; for (size_t i = 0; i < 100; i++) actualMean = actualMean + Sphere2::Random(rng).point3(); - actualMean = actualMean/100; + actualMean = actualMean / 100; EXPECT(assert_equal(expectedMean,actualMean,0.1)); }