Named constructor FromPoint3, with optional Jacobian

gtsam/geometry/Sphere2.cpp

@@ -27,6 +27,21 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+Sphere2 Sphere2::FromPoint3(const Point3& point,
+    boost::optional<Matrix&> 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 :-(

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<Matrix&> 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

gtsam/geometry/tests/testSphere2.cpp

@@ -25,6 +25,7 @@
 #include <boost/foreach.hpp>
 #include <boost/random.hpp>
 #include <boost/assign/std/vector.hpp>
+#include <cmath>
 
 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<Sphere2, Point3>(
+      boost::bind(Sphere2::FromPoint3, _1, boost::none), point);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
 /* ************************************************************************* */