From c94d5aba26cc5fcc9f1a58671654915b36dc97b4 Mon Sep 17 00:00:00 2001 From: Alex Trevor Date: Fri, 10 Jan 2014 15:40:36 -0500 Subject: [PATCH] Unit tests have been added to cover both coordinate modes, and specify mode where appropriate. Sphere2 expmap/logmap now properly handle antipodal points. --- gtsam/geometry/Sphere2.cpp | 16 +++++--- gtsam/geometry/Sphere2.h | 7 ++++ gtsam/geometry/tests/testSphere2.cpp | 60 ++++++++++++++++++++++++---- 3 files changed, 70 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index 4049432e4..ca85806f3 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -109,12 +109,16 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const { 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); // Avoid nan - if (xi_hat_norm == 0.0) - return Sphere2 (point3 ()); - + 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; @@ -140,8 +144,10 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const { double theta = acos(p.transpose() * q); // the below will be nan if theta == 0.0 - if (theta == 0.0) + if (p == q) return (Vector (2) << 0, 0); + else if (p == -q) + return (Vector (2) << M_PI, 0); Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta)); Vector result = B.transpose() * result_hat; diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 18f3a789d..9474f0867 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -102,6 +102,13 @@ public: return p_; } + /// Return unit-norm Vector + Vector unitVector(boost::optional H = boost::none) const { + if (H) + *H = basis(); + return (p_.vector ()); + } + /// Signed, vector-valued error between two directions Vector error(const Sphere2& q, boost::optional H = boost::none) const; diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index b64a00f62..d64c0a7a8 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -81,9 +81,9 @@ static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) { } TEST(Sphere2, unrotate) { - Rot3 R = Rot3::yaw(-M_PI/2.0); + Rot3 R = Rot3::yaw(-M_PI/4.0); Sphere2 p(1, 0, 0); - Sphere2 expected = Sphere2(0, 1, 0); + Sphere2 expected = Sphere2(1, 1, 0); Sphere2 actual = R.unrotate (p); EXPECT(assert_equal(expected, actual, 1e-8)); Matrix actualH, expectedH; @@ -102,8 +102,8 @@ TEST(Sphere2, unrotate) { //******************************************************************************* TEST(Sphere2, error) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); + 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); 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)); @@ -126,8 +126,8 @@ TEST(Sphere2, error) { //******************************************************************************* TEST(Sphere2, distance) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); + 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); 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); @@ -171,9 +171,20 @@ TEST(Sphere2, retract) { Vector v(2); v << 0.5, 0; Sphere2 expected(Point3(1, 0, 0.5)); - Sphere2 actual = p.retract(v); + Sphere2 actual = p.retract(v, Sphere2::RENORM); EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8)); +} + +//******************************************************************************* +TEST(Sphere2, retract_expmap) { + Sphere2 p; + Vector v(2); + v << (M_PI/2.0), 0; + Sphere2 expected(Point3(0, 0, 1)); + Sphere2 actual = p.retract(v, Sphere2::EXPMAP); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8)); } //******************************************************************************* @@ -223,6 +234,39 @@ 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); + for (size_t i = 0; i < numIterations; i++) { + + // Sleep for the random number generator (TODO?: Better create all of them first). + sleep(0); + + // Create the two Sphere2s. + // Unlike the above case, we can use any two sphers. + 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) + v12 = v12 / M_PI; + Sphere2 s2 = s1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = s1.localCoordinates(s2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + Sphere2 actual_s2 = s1.retract(actual_v12); + EXPECT(assert_equal(s2, actual_s2, 1e-3)); + } +} + //******************************************************************************* //TEST( Pose2, between ) //{