From a47f0a30dadcacdb5a56e85865b553b9a46e09d8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@dellaert.com>
Date: Thu, 26 Dec 2013 13:59:44 -0500
Subject: [PATCH] Use Sphere2::Random to generate a random Rot3

---
 gtsam/geometry/Rot3.cpp | 10 ++++++++++
 gtsam/geometry/Rot3.h   |  3 +++
 2 files changed, 13 insertions(+)

diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 0a3b4fd9f..8dcf14d4b 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -20,6 +20,7 @@
 
 #include <gtsam/geometry/Rot3.h>
 #include <boost/math/constants/constants.hpp>
+#include <boost/random.hpp>
 #include <cmath>
 
 using namespace std;
@@ -43,6 +44,15 @@ Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
   return rodriguez(w.point3(),theta);
 }
 
+/* ************************************************************************* */
+Rot3 Rot3::Random(boost::random::mt19937 & rng) {
+  // TODO allow any engine without including all of boost :-(
+  Sphere2 w = Sphere2::Random(rng);
+  boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
+  double angle = randomAngle(rng);
+  return rodriguez(w,angle);
+}
+
 /* ************************************************************************* */
 Rot3 Rot3::rodriguez(const Vector& w) {
   double t = w.norm();
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 5bb382a3e..6fb3aea8a 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -106,6 +106,9 @@ namespace gtsam {
      */
     Rot3(const Quaternion& q);
 
+    /// Random, generates a random axis, then random angle \in [-p,pi]
+    static Rot3 Random(boost::random::mt19937 & rng);
+
     /** Virtual destructor */
     virtual ~Rot3() {}