From c10f1954921bea36baf0fda3ebc6b10e0ca0e5da Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 22 Feb 2022 22:42:14 -0800 Subject: [PATCH 1/6] use rng in TA initialization --- gtsam/sfm/TranslationRecovery.cpp | 17 +++++++++++++---- gtsam/sfm/TranslationRecovery.h | 12 ++++++++++-- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index f38c14ba7c..aa7b14709b 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 c99836853d..430b54d1d0 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 initializeRandomly(std::mt19937 &rng) const; + + /** + * @brief Version of initializeRandomly with a fixed seed. + * * @return Values */ - Values initalizeRandomly() const; + Values initializeRandomly() const; /** * @brief Build and optimize factor graph. From 24173ab025badc7a01d4828a76280d580145ae85 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 22 Feb 2022 22:42:42 -0800 Subject: [PATCH 2/6] update comment in shonan --- gtsam/sfm/ShonanAveraging.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index de12de4782..8be16e2042 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; From 93519e4ccaca89081e20380bdcebf0fe3ff05a32 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 24 Feb 2022 23:22:05 -0800 Subject: [PATCH 3/6] updating tolerance in tests --- tests/testTranslationRecovery.cpp | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 2915a375e8..090f2e1cb2 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -116,8 +116,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) { @@ -153,9 +153,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) { @@ -190,9 +190,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) { @@ -231,10 +231,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) { @@ -261,9 +261,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)); } /* ************************************************************************* */ From 1a2eb9a1018c5c060f1fbb051ff2f149d91101d2 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 28 Feb 2022 22:21:59 -0800 Subject: [PATCH 4/6] change to pointer --- gtsam/sfm/TranslationRecovery.cpp | 4 ++-- gtsam/sfm/TranslationRecovery.h | 16 ++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index aa7b14709b..9d72f56dab 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -91,7 +91,7 @@ void TranslationRecovery::addPrior( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); } -Values TranslationRecovery::initializeRandomly(std::mt19937 &rng) 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. @@ -121,7 +121,7 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 &rng) const { } Values TranslationRecovery::initializeRandomly() const { - return initializeRandomly(kRandomNumberGenerator); + return initializeRandomly(&kRandomNumberGenerator); } Values TranslationRecovery::run(const double scale) const { diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 430b54d1d0..30c9a14e39 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -16,16 +16,16 @@ * @brief Recovering translations in an epipolar graph when rotations are given. */ -#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 @@ -99,15 +99,15 @@ class TranslationRecovery { /** * @brief Create random initial translations. - * + * * @param rng random number generator * @return Values */ - Values initializeRandomly(std::mt19937 &rng) const; + Values initializeRandomly(std::mt19937 *rng) const; /** * @brief Version of initializeRandomly with a fixed seed. - * + * * @return Values */ Values initializeRandomly() const; From 42bee1ab0baf3a303576dd44286104b2a3103917 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 28 Feb 2022 22:23:41 -0800 Subject: [PATCH 5/6] lamda capture all variables --- gtsam/sfm/TranslationRecovery.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 9d72f56dab..64953d2a5e 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -96,7 +96,7 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { // Create a lambda expression that checks whether value exists and randomly // initializes if not. Values initial; - auto insert = [&initial, &rng, &randomVal](Key j) { + auto insert = [&](Key j) { if (!initial.exists(j)) { initial.insert( j, Point3(randomVal(rng), randomVal(rng), randomVal(rng))); From 97ee1268a28cd82811d9acd660544b43a4edd24b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 28 Feb 2022 22:27:28 -0800 Subject: [PATCH 6/6] update pointer usage --- gtsam/sfm/TranslationRecovery.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 64953d2a5e..2e81c2d561 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -99,7 +99,7 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { auto insert = [&](Key j) { if (!initial.exists(j)) { initial.insert( - j, Point3(randomVal(rng), randomVal(rng), randomVal(rng))); + j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); } };