Use Sphere2::Random to generate a random Rot3

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();

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() {}