From dc8236805f9b52e70f15dfe1aab3eabd22ace9c4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@dellaert.com>
Date: Thu, 26 Dec 2013 13:59:15 -0500
Subject: [PATCH] Generating random directions. Somewhat clumsy use of forward
 declaration, issue #19 applies as well.

---
 .cproject                            |  8 ++++++++
 gtsam/geometry/Sphere2.cpp           | 14 +++++++++++++-
 gtsam/geometry/Sphere2.h             | 15 +++++++++++++++
 gtsam/geometry/tests/testSphere2.cpp | 20 ++++++++++++++++++--
 4 files changed, 54 insertions(+), 3 deletions(-)

diff --git a/.cproject b/.cproject
index 19be6b629..7398f3d12 100644
--- a/.cproject
+++ b/.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>
diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp
index a5c8c07ea..b6cae287c 100644
--- a/gtsam/geometry/Sphere2.cpp
+++ b/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);
diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h
index 5d1bbd7b2..0c549ed0b 100644
--- a/gtsam/geometry/Sphere2.h
+++ b/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
diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp
index af435bcea..c6e519a44 100644
--- a/gtsam/geometry/tests/testSphere2.cpp
+++ b/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));