From a2f99ac71cbaad5b92dad26ff2f72c4ca872a7ca Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Fri, 5 Mar 2021 07:21:38 +0000 Subject: [PATCH 1/2] unit test + fix segfault --- gtsam/sfm/TranslationRecovery.cpp | 10 ++++++++++ tests/testTranslationRecovery.cpp | 29 +++++++++++++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index d4100b00a8..7f788ff84c 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -81,6 +81,7 @@ void TranslationRecovery::addPrior( const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); + if(edge == relativeTranslations_.end()) return; graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); graph->emplace_shared >( @@ -102,6 +103,15 @@ Values TranslationRecovery::initalizeRandomly() const { insert(edge.key1()); insert(edge.key2()); } + + // If there are no valid edges, but zero-distance edges exist, initialize one of + // the nodes in a connected component of zero-distance edges. + if(initial.empty() && !sameTranslationNodes_.empty()){ + for(const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { + Key optimizedKey = optimizedAndDuplicateKeys.first; + initial.insert(optimizedKey, Point3(0, 0, 0)); + } + } return initial; } diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 7260fd5af1..9a4e2b5458 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -238,6 +238,35 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { EXPECT(assert_equal(Point3(2, -2, 0), result.at(3))); } +TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { + Values poses; + poses.insert(0, Pose3(Rot3::RzRyRx(-M_PI / 6, 0, 0), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}, {2, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + // Graph size will be zero as there no 'non-zero distance' edges. + EXPECT_LONGS_EQUAL(0, graph.size()); + + // Run translation recovery + 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))); +} + + /* ************************************************************************* */ int main() { TestResult tr; From 759dcaa1dd9bd799c6a531561563acfa9ae5032b Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 5 Mar 2021 00:06:30 -0800 Subject: [PATCH 2/2] formatting changes --- gtsam/sfm/TranslationRecovery.cpp | 10 +++++----- tests/testTranslationRecovery.cpp | 17 ++++++++--------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 7f788ff84c..f38c14ba7c 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -81,7 +81,7 @@ void TranslationRecovery::addPrior( const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); - if(edge == relativeTranslations_.end()) return; + if (edge == relativeTranslations_.end()) return; graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); graph->emplace_shared >( @@ -104,10 +104,10 @@ Values TranslationRecovery::initalizeRandomly() const { insert(edge.key2()); } - // If there are no valid edges, but zero-distance edges exist, initialize one of - // the nodes in a connected component of zero-distance edges. - if(initial.empty() && !sameTranslationNodes_.empty()){ - for(const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { + // If there are no valid edges, but zero-distance edges exist, initialize one + // of the nodes in a connected component of zero-distance edges. + if (initial.empty() && !sameTranslationNodes_.empty()) { + for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { Key optimizedKey = optimizedAndDuplicateKeys.first; initial.insert(optimizedKey, Point3(0, 0, 0)); } diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 9a4e2b5458..2915a375e8 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -17,7 +17,6 @@ */ #include - #include #include @@ -185,7 +184,7 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); // There is only 1 non-zero translation edge. - EXPECT_LONGS_EQUAL(1, graph.size()); + EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery const auto result = algorithm.run(/*scale=*/3.0); @@ -244,28 +243,28 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { poses.insert(1, Pose3(Rot3(), Point3(0, 0, 0))); poses.insert(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0))); - auto relativeTranslations = TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}, {2, 0}}); + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 2}, {2, 0}}); // Check simulated measurements. for (auto& unitTranslation : relativeTranslations) { - EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), - unitTranslation.measured())); + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); } TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); - // Graph size will be zero as there no 'non-zero distance' edges. + // Graph size will be zero as there no 'non-zero distance' edges. EXPECT_LONGS_EQUAL(0, graph.size()); - + // Run translation recovery 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))); } - /* ************************************************************************* */ int main() {