diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index a5c31534c..e035da4c7 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/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; diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index f38c14ba7..2e81c2d56 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 = [&](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 620a1c130..30c9a14e3 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -16,18 +16,16 @@ * @brief Recovering translations in an epipolar graph when rotations are given. */ -#pragma once +#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include - 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. diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 4fe643047..833f11355 100644 --- a/tests/testTranslationRecovery.cpp +++ b/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(0))); - EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(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(0))); - EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at(3))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at(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(0))); - EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(3, 0, 0), result.at(2))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(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(0))); - EXPECT(assert_equal(Point3(4, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(4, 0, 0), result.at(2))); - EXPECT(assert_equal(Point3(2, -2, 0), result.at(3))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(2), 1e-8)); + EXPECT(assert_equal(Point3(2, -2, 0), result.at(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(0))); - EXPECT(assert_equal(Point3(0, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(0, 0, 0), result.at(2))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(2), 1e-8)); } /* ************************************************************************* */