diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index e86121537..7729bd354 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -153,14 +153,14 @@ Unit3 Unit3::retract(const Vector2& v) const { /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); - // Crucial quantitity here is y = theta/sin(theta) with theta=acos(x) + // Crucial quantity here is y = theta/sin(theta) with theta=acos(x) // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2) - // We treat the special caes 1 and -1 below + // We treat the special case 1 and -1 below const double x2 = x * x; const double z = 1 - x2; double y; if (z < std::numeric_limits::epsilon()) { - if (x > 0) // expand at x=1 + if (x > 0) // first order expansion at x=1 y = 1.0 - (x - 1.0) / 3.0; else // cop out return Vector2(M_PI, 0.0); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a4227acdf..e55caaa3c 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -192,6 +192,16 @@ TEST(Unit3, localCoordinates) { EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-8)); } + { + Unit3 p(0,1,0), q(0,-1,0); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + } + { + Unit3 p(0,0,1), q(0,0,-1); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + } double twist = 1e-4; {