diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index b6cae287c..29a170c4d 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -27,6 +27,21 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +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: + Matrix D_p_point; + point.normalize(D_p_point); // TODO, this calculates norm a second time :-( + // Calculate the 2*3 Jacobian + H->resize(2, 3); + *H << direction.basis().transpose() * D_p_point; + } + return direction; +} + /* ************************************************************************* */ Sphere2 Sphere2::Random(boost::random::mt19937 & rng) { // TODO allow any engine without including all of boost :-( diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 0c549ed0b..8555b5d30 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -65,6 +65,10 @@ public: p_ = p_ / p_.norm(); } + /// Named constructor from Point3 with optional Jacobian + 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); @@ -85,7 +89,11 @@ public: /// @name Other functionality /// @{ - /// Returns the local coordinate frame to tangent plane + /** + * Returns the local coordinate frame to tangent plane + * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions + * tangent to the sphere at the current direction. + */ Matrix basis() const; /// Return skew-symmetric associated with 3D point on unit sphere diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index c6e519a44..b9aa84949 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -25,6 +25,7 @@ #include #include #include +#include using namespace boost::assign; using namespace gtsam; @@ -244,13 +245,24 @@ TEST(Sphere2, Random) { // Check that is deterministic given same random seed Point3 expected(-0.667578, 0.671447, 0.321713); Point3 actual = Sphere2::Random(rng).point3(); - EXPECT(assert_equal(expected,actual,1e-5)); + EXPECT(assert_equal(expected, actual, 1e-5)); // Check that means are all zero at least Point3 expectedMean, actualMean; for (size_t i = 0; i < 100; i++) actualMean = actualMean + Sphere2::Random(rng).point3(); - actualMean = actualMean/100; - EXPECT(assert_equal(expectedMean,actualMean,0.1)); + actualMean = actualMean / 100; + EXPECT(assert_equal(expectedMean, actualMean, 0.1)); +} + +//************************************************************************* +TEST (Sphere2, FromPoint3) { + Matrix actualH; + Point3 point(1, -2, 3); // arbitrary point + Sphere2 expected(point); + EXPECT(assert_equal(expected, Sphere2::FromPoint3(point, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(Sphere2::FromPoint3, _1, boost::none), point); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); } /* ************************************************************************* */