Generating random directions. Somewhat clumsy use of forward declaration, issue #19 applies as well.

.cproject

@@ -445,6 +445,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSphere2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testSphere2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam/geometry/Sphere2.cpp

@@ -19,12 +19,24 @@
 
 #include <gtsam/geometry/Sphere2.h>
 #include <gtsam/geometry/Point2.h>
+#include <boost/random/mersenne_twister.hpp>
+#include <boost/random/uniform_on_sphere.hpp>
 #include <iostream>
 
 using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
+  // TODO allow any engine without including all of boost :-(
+  boost::random::uniform_on_sphere<double> randomDirection(3);
+  vector<double> d = randomDirection(rng);
+  Sphere2 result;
+  result.p_ = Point3(d[0], d[1], d[2]);
+  return result;
+}
+
 /* ************************************************************************* */
 Matrix Sphere2::basis() const {
 
@@ -78,7 +90,7 @@ Vector Sphere2::error(const Sphere2& q, boost::optional<Matrix&> H) const {
 
 /* ************************************************************************* */
 double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
-  Vector xi = error(q,H);
+  Vector xi = error(q, H);
   double theta = xi.norm();
   if (H)
     *H = (xi.transpose() / theta) * (*H);

gtsam/geometry/Sphere2.h

@@ -22,6 +22,18 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/DerivedValue.h>
 
+// (Cumbersome) forward declaration for random generator
+namespace boost {
+namespace random {
+template<class UIntType, std::size_t w, std::size_t n, std::size_t m,
+    std::size_t r, UIntType a, std::size_t u, UIntType d, std::size_t s,
+    UIntType b, std::size_t t, UIntType c, std::size_t l, UIntType f>
+class mersenne_twister_engine;
+typedef mersenne_twister_engine<uint32_t, 32, 624, 397, 31, 0x9908b0df, 11,
+    0xffffffff, 7, 0x9d2c5680, 15, 0xefc60000, 18, 1812433253> mt19937;
+}
+}
+
 namespace gtsam {
 
 /// Represents a 3D point on a unit sphere.
@@ -53,6 +65,9 @@ public:
     p_ = p_ / p_.norm();
   }
 
+  /// Random direction, using boost::uniform_on_sphere
+  static Sphere2 Random(boost::random::mt19937 & rng);
+
   /// @}
 
   /// @name Testable

gtsam/geometry/tests/testSphere2.cpp

@@ -23,6 +23,7 @@
 #include <CppUnitLite/TestHarness.h>
 #include <boost/bind.hpp>
 #include <boost/foreach.hpp>
+#include <boost/random.hpp>
 #include <boost/assign/std/vector.hpp>
 
 using namespace boost::assign;
@@ -63,12 +64,12 @@ TEST(Sphere2, rotate) {
   Matrix actualH, expectedH;
   // Use numerical derivatives to calculate the expected Jacobian
   {
-    expectedH = numericalDerivative21(rotate_,R,p);
+    expectedH = numericalDerivative21(rotate_, R, p);
     R.rotate(p, actualH, boost::none);
     EXPECT(assert_equal(expectedH, actualH, 1e-9));
   }
   {
-    expectedH = numericalDerivative22(rotate_,R,p);
+    expectedH = numericalDerivative22(rotate_, R, p);
     R.rotate(p, boost::none, actualH);
     EXPECT(assert_equal(expectedH, actualH, 1e-9));
   }
@@ -237,6 +238,21 @@ TEST(Sphere2, localCoordinates_retract) {
 //
 //}
 
+//*******************************************************************************
+TEST(Sphere2, Random) {
+  boost::random::mt19937 rng(42);
+  // 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));
+  // 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));
+}
+
 /* ************************************************************************* */
 int main() {
   srand(time(NULL));