Merge pull request #1120 from borglab/ta-seed

Using a random number generator in translation averaging

gtsam/sfm/ShonanAveraging.h

@@ -300,6 +300,7 @@ class GTSAM_EXPORT ShonanAveraging {
   /**
    * Create initial Values of type SO(p)
    * @param p the dimensionality of the rotation manifold
+   * @param rng random number generator
    */
   Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
 

gtsam/sfm/TranslationRecovery.cpp

@@ -35,6 +35,9 @@
 using namespace gtsam;
 using namespace std;
 
+// In Wrappers we have no access to this so have a default ready.
+static std::mt19937 kRandomNumberGenerator(42);
+
 TranslationRecovery::TranslationRecovery(
     const TranslationRecovery::TranslationEdges &relativeTranslations,
     const LevenbergMarquardtParams &lmParams)
@@ -88,13 +91,15 @@ void TranslationRecovery::addPrior(
       edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
 }
 
-Values TranslationRecovery::initalizeRandomly() const {
+Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
+  uniform_real_distribution<double> randomVal(-1, 1);
   // Create a lambda expression that checks whether value exists and randomly
   // initializes if not.
   Values initial;
-  auto insert = [&initial](Key j) {
+  auto insert = [&](Key j) {
     if (!initial.exists(j)) {
-      initial.insert<Point3>(j, Vector3::Random());
+      initial.insert<Point3>(
+          j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
     }
   };
 
@@ -115,10 +120,14 @@ Values TranslationRecovery::initalizeRandomly() const {
   return initial;
 }
 
+Values TranslationRecovery::initializeRandomly() const {
+  return initializeRandomly(&kRandomNumberGenerator);
+}
+
 Values TranslationRecovery::run(const double scale) const {
   auto graph = buildGraph();
   addPrior(scale, &graph);
-  const Values initial = initalizeRandomly();
+  const Values initial = initializeRandomly();
   LevenbergMarquardtOptimizer lm(graph, initial, params_);
   Values result = lm.optimize();
 

gtsam/sfm/TranslationRecovery.h

@@ -16,18 +16,16 @@
  * @brief Recovering translations in an epipolar graph when rotations are given.
  */
 
-#pragma once
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/BinaryMeasurement.h>
 
 #include <map>
 #include <set>
 #include <utility>
 #include <vector>
 
-#include <gtsam/geometry/Unit3.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/sfm/BinaryMeasurement.h>
-
 namespace gtsam {
 
 // Set up an optimization problem for the unknown translations Ti in the world
@@ -102,9 +100,17 @@ class TranslationRecovery {
   /**
    * @brief Create random initial translations.
    *
+   * @param rng random number generator
    * @return Values
    */
-  Values initalizeRandomly() const;
+  Values initializeRandomly(std::mt19937 *rng) const;
+
+  /**
+   * @brief Version of initializeRandomly with a fixed seed.
+   *
+   * @return Values
+   */
+  Values initializeRandomly() const;
 
   /**
    * @brief Build and optimize factor graph.

tests/testTranslationRecovery.cpp

@@ -115,8 +115,8 @@ TEST(TranslationRecovery, TwoPoseTest) {
   const auto result = algorithm.run(/*scale=*/3.0);
 
   // Check result for first two translations, determined by prior
-  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
-  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
 }
 
 TEST(TranslationRecovery, ThreePoseTest) {
@@ -152,9 +152,9 @@ TEST(TranslationRecovery, ThreePoseTest) {
   const auto result = algorithm.run(/*scale=*/3.0);
 
   // Check result
-  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
-  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
-  EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at<Point3>(3)));
+  EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at<Point3>(3), 1e-8));
 }
 
 TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
@@ -189,9 +189,9 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
   const auto result = algorithm.run(/*scale=*/3.0);
 
   // Check result
-  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
-  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
-  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(2)));
+  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(2), 1e-8));
 }
 
 TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
@@ -230,10 +230,10 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
   const auto result = algorithm.run(/*scale=*/4.0);
 
   // Check result
-  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
-  EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(1), 1e-8));
-  EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(2)));
+  EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(2), 1e-8));
-  EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3)));
+  EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3), 1e-8));
 }
 
 TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
@@ -260,9 +260,9 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
   const auto result = algorithm.run(/*scale=*/4.0);
 
   // Check result
-  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
-  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(1), 1e-8));
-  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2)));
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2), 1e-8));
 }
 
 /* ************************************************************************* */