diff --git a/.cproject b/.cproject
index 19be6b629..7398f3d12 100644
--- a/.cproject
+++ b/.cproject
@@ -445,6 +445,14 @@
true
true
+
+ make
+ -j5
+ testSphere2.run
+ true
+ true
+ true
+
make
-j2
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
#include
+#include
+#include
#include
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 randomDirection(3);
+ vector 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 H) const {
/* ************************************************************************* */
double Sphere2::distance(const Sphere2& q, boost::optional 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
#include
+// (Cumbersome) forward declaration for random generator
+namespace boost {
+namespace random {
+template
+class mersenne_twister_engine;
+typedef mersenne_twister_engine 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
#include
#include
+#include
#include
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));