diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index f38c14ba7..aa7b14709 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/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 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 = [&initial, &rng, &randomVal](Key j) { if (!initial.exists(j)) { - initial.insert(j, Vector3::Random()); + initial.insert( + 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(); diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index c99836853..430b54d1d 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -99,10 +99,18 @@ 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.