diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index 7b9c53822..356fb8a36 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -80,17 +80,24 @@ Sphere2 Sphere2::Rotate(const Rot3& R, const Sphere2& p, /* ************************************************************************* */ Sphere2 operator*(const Rot3& R, const Sphere2& p) { - return Sphere2::Rotate(R,p); + return Sphere2::Rotate(R, p); } /* ************************************************************************* */ -double Sphere2::distance(const Sphere2& other, - boost::optional H) const { +Vector Sphere2::error(const Sphere2& q, boost::optional H) const { Matrix Bt = basis().transpose(); - Vector xi = Bt * other.p_.vector(); + Vector xi = Bt * q.p_.vector(); + if (H) + *H = Bt * q.basis(); + return xi; +} + +/* ************************************************************************* */ +double Sphere2::distance(const Sphere2& q, boost::optional H) const { + Vector xi = error(q,H); double theta = xi.norm(); if (H) - *H = (xi.transpose() / theta) * Bt * other.basis(); + *H = (xi.transpose() / theta) * (*H); return theta; } diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 43c2c5cca..4a06f052c 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -84,8 +84,12 @@ public: /// Rotate sugar friend Sphere2 operator*(const Rot3& R, const Sphere2& p); + /// Signed, vector-valued error between two directions + Vector error(const Sphere2& q, + boost::optional H = boost::none) const; + /// Distance between two directions - double distance(const Sphere2& other, + double distance(const Sphere2& q, boost::optional H = boost::none) const; /// @} diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index 1b8ca68ae..5802d48a2 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -51,6 +51,30 @@ TEST(Sphere2, rotate) { } } +//******************************************************************************* +TEST(Sphere2, error) { + 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)); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative11( + boost::bind(&Sphere2::error, &p, _1, boost::none), q); + p.error(q, actual); + EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + } + { + expected = numericalDerivative11( + boost::bind(&Sphere2::error, &p, _1, boost::none), r); + p.error(r, actual); + EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + } +} + //******************************************************************************* TEST(Sphere2, distance) { Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //