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