12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Use Sphere2::Random to generate a random Rot3

release/4.3a0
Frank Dellaert 2013-12-26 13:59:44 -05:00
parent dc8236805f
commit a47f0a30da
2 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Rot3.cpp
View File

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

3
gtsam/geometry/Rot3.h
View File

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