From 4bc250e7c04c4b91a3f15715d0e70dd82ede388b Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sat, 28 Nov 2020 23:44:20 -0800 Subject: [PATCH 1/4] new test doesnt pass --- gtsam/sfm/TranslationRecovery.cpp | 53 +++++++++++++--- gtsam/sfm/TranslationRecovery.h | 6 +- tests/testTranslationRecovery.cpp | 100 +++++++++++++++++++++++++++--- 3 files changed, 141 insertions(+), 18 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 319129840..8821d490b 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -16,8 +16,7 @@ * @brief Source code for recovering translations when rotations are given */ -#include - +#include #include #include #include @@ -27,11 +26,39 @@ #include #include #include +#include #include +#include +#include + using namespace gtsam; using namespace std; +TranslationRecovery::TranslationRecovery( + const TranslationRecovery::TranslationEdges &relativeTranslations, + const LevenbergMarquardtParams &lmParams) + : params_(lmParams) { + TranslationEdges tempRelativeTranslations; + DSFMap sameTranslationDSF; + + for (const auto &edge : relativeTranslations) { + Key key1 = sameTranslationDSF.find(edge.key1()); + Key key2 = sameTranslationDSF.find(edge.key2()); + if (key1 != key2 && edge.measured().equals(Unit3(0.0, 0.0, 0.0))) { + sameTranslationDSF.merge(key1, key2); + } + } + for (const auto &edge : relativeTranslations) { + Key key1 = sameTranslationDSF.find(edge.key1()); + Key key2 = sameTranslationDSF.find(edge.key2()); + if (key1 == key2) continue; + relativeTranslations_.emplace_back(key1, key2, edge.measured(), + edge.noiseModel()); + } + sameTranslationNodes_ = sameTranslationDSF.sets(); +} + NonlinearFactorGraph TranslationRecovery::buildGraph() const { NonlinearFactorGraph graph; @@ -44,13 +71,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { return graph; } -void TranslationRecovery::addPrior(const double scale, - NonlinearFactorGraph *graph, - const SharedNoiseModel &priorNoiseModel) const { +void TranslationRecovery::addPrior( + const double scale, NonlinearFactorGraph *graph, + const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); - graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); - graph->emplace_shared >(edge->key2(), scale * edge->measured().point3(), - edge->noiseModel()); + graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), + priorNoiseModel); + graph->emplace_shared >( + edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); } Values TranslationRecovery::initalizeRandomly() const { @@ -77,6 +105,15 @@ Values TranslationRecovery::run(const double scale) const { const Values initial = initalizeRandomly(); LevenbergMarquardtOptimizer lm(graph, initial, params_); Values result = lm.optimize(); + + for (const auto &sameTranslationKeys : sameTranslationNodes_) { + Key optimizedKey = sameTranslationKeys.first; + std::set duplicateKeys = sameTranslationKeys.second; + for (const Key duplicateKey : duplicateKeys) { + if (result.exists(duplicateKey)) continue; + result.insert(duplicateKey, result.at(optimizedKey)); + } + } return result; } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index d5538f91b..c3492d067 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -23,6 +23,8 @@ #include #include +#include +#include namespace gtsam { @@ -54,6 +56,7 @@ class TranslationRecovery { private: TranslationEdges relativeTranslations_; LevenbergMarquardtParams params_; + std::map> sameTranslationNodes_; public: /** @@ -67,8 +70,7 @@ class TranslationRecovery { * default LM parameters. */ TranslationRecovery(const TranslationEdges &relativeTranslations, - const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()) - : relativeTranslations_(relativeTranslations), params_(lmParams) {} + const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()); /** * @brief Build the factor graph to do the optimization. diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index eb34ba803..84ed577f1 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -16,9 +16,8 @@ * @brief test recovering translations when rotations are given. */ -#include - #include +#include #include using namespace std; @@ -49,14 +48,14 @@ TEST(TranslationRecovery, BAL) { poses, {{0, 1}, {0, 2}, {1, 2}}); // Check - Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test - for(auto& unitTranslation : relativeTranslations) { - const Pose3 wTa = poses.at(unitTranslation.key1()), + Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test + for (auto& unitTranslation : relativeTranslations) { + const Pose3 wTa = poses.at(unitTranslation.key1()), wTb = poses.at(unitTranslation.key2()); const Point3 Ta = wTa.translation(), Tb = wTb.translation(); const Unit3 w_aZb = unitTranslation.measured(); EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); - if(unitTranslation.key1() == 0 && unitTranslation.key2() == 1) { + if (unitTranslation.key1() == 0 && unitTranslation.key2() == 1) { w_aZb_stored = unitTranslation.measured(); } } @@ -77,14 +76,99 @@ TEST(TranslationRecovery, BAL) { Point3 Ta = poses.at(0).translation(); Point3 Tb = poses.at(1).translation(); Point3 Tc = poses.at(2).translation(); - Point3 expected = - (Tc - Ta) * (scale / (Tb - Ta).norm()); + Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); EXPECT(assert_equal(expected, result.at(2), 1e-4)); // TODO(frank): how to get stats back? // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); } +TEST(TranslationRecovery, ZeroRelativeTranslations) { + // Create a graph with 3 cameras. + // __ __ + // \/ \/ + // 0 _____ 1 + // 2 <| + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with very little FOV overlap. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3())); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}}); + + // Check + for (auto& unitTranslation : relativeTranslations) { + const Pose3 wTa = poses.at(unitTranslation.key1()), + wTb = poses.at(unitTranslation.key2()); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + const Unit3 w_aZb = unitTranslation.measured(); + EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(1, graph.size()); + + // Translation recovery, version 1 + const double scale = 2.0; + const auto result = algorithm.run(scale); + + // Check result for first two translations, determined by prior + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(2))); +} + +TEST(TranslationRecovery, ZeroRelativeTranslations4Cameras) { + // Create a graph with 4 cameras. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ 2 <| + // \ / + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with very little FOV overlap. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3())); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}}); + + // Check + for (auto& unitTranslation : relativeTranslations) { + const Pose3 wTa = poses.at(unitTranslation.key1()), + wTb = poses.at(unitTranslation.key2()); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + const Unit3 w_aZb = unitTranslation.measured(); + EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(3, graph.size()); + + // Translation recovery, version 1 + const double scale = 2.0; + const auto result = algorithm.run(scale); + + // Check result for first two translations, determined by prior + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(2))); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3))); +} + /* ************************************************************************* */ int main() { TestResult tr; From 8d009c2fcf55e41ac7c7b7f30859322cf47bcb67 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Mon, 30 Nov 2020 00:30:19 -0800 Subject: [PATCH 2/4] translation recovery unit tests pass --- gtsam/sfm/TranslationRecovery.cpp | 2 +- gtsam/sfm/TranslationRecovery.h | 25 ++--- tests/testTranslationRecovery.cpp | 167 +++++++++++++++++++++--------- 3 files changed, 132 insertions(+), 62 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 8821d490b..8d27136e3 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -11,7 +11,7 @@ /** * @file TranslationRecovery.cpp - * @author Frank Dellaert + * @author Frank Dellaert, Akshay Krishnan * @date March 2020 * @brief Source code for recovering translations when rotations are given */ diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index c3492d067..9ffe45685 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 @@ -63,14 +63,15 @@ class TranslationRecovery { * @brief Construct a new Translation Recovery object * * @param relativeTranslations the relative translations, in world coordinate - * frames, vector of BinaryMeasurements of Unit3, where each key of a measurement - * is a point in 3D. + * frames, vector of BinaryMeasurements of Unit3, where each key of a + * measurement is a point in 3D. * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be * used to modify the parameters for the LM optimizer. By default, uses the - * default LM parameters. + * default LM parameters. */ - TranslationRecovery(const TranslationEdges &relativeTranslations, - const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()); + TranslationRecovery( + const TranslationEdges &relativeTranslations, + const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()); /** * @brief Build the factor graph to do the optimization. @@ -110,8 +111,8 @@ class TranslationRecovery { * * @param poses SE(3) ground truth poses stored as Values * @param edges pairs (a,b) for which a measurement w_aZb will be generated. - * @return TranslationEdges vector of binary measurements where the keys are - * the cameras and the measurement is the simulated Unit3 translation + * @return TranslationEdges vector of binary measurements where the keys are + * the cameras and the measurement is the simulated Unit3 translation * direction between the cameras. */ static TranslationEdges SimulateMeasurements( diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 84ed577f1..e4fbd9219 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -11,18 +11,29 @@ /** * @file testTranslationRecovery.cpp - * @author Frank Dellaert + * @author Frank Dellaert, Akshay Krishnan * @date March 2020 * @brief test recovering translations when rotations are given. */ #include + #include #include using namespace std; using namespace gtsam; +// Returns the Unit3 direction as measured in the binary measurement, but +// computed from the input poses. Helper function used in the unit tests. +Unit3 GetDirectionFromPoses(const Values& poses, + const BinaryMeasurement& unitTranslation) { + const Pose3 wTa = poses.at(unitTranslation.key1()), + wTb = poses.at(unitTranslation.key2()); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + return Unit3(Tb - Ta); +} + /* ************************************************************************* */ // We read the BAL file, which has 3 cameras in it, with poses. We then assume // the rotations are correct, but translations have to be estimated from @@ -47,30 +58,25 @@ TEST(TranslationRecovery, BAL) { const auto relativeTranslations = TranslationRecovery::SimulateMeasurements( poses, {{0, 1}, {0, 2}, {1, 2}}); - // Check - Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test + // Check simulated measurements. for (auto& unitTranslation : relativeTranslations) { - const Pose3 wTa = poses.at(unitTranslation.key1()), - wTb = poses.at(unitTranslation.key2()); - const Point3 Ta = wTa.translation(), Tb = wTb.translation(); - const Unit3 w_aZb = unitTranslation.measured(); - EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); - if (unitTranslation.key1() == 0 && unitTranslation.key2() == 1) { - w_aZb_stored = unitTranslation.measured(); - } + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); } TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); EXPECT_LONGS_EQUAL(3, graph.size()); - // Translation recovery, version 1 + // Run translation recovery const double scale = 2.0; const auto result = algorithm.run(scale); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at(1))); + EXPECT(assert_equal( + Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])), + result.at(1))); // Check that the third translations is correct Point3 Ta = poses.at(0).translation(); @@ -83,53 +89,120 @@ TEST(TranslationRecovery, BAL) { // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); } -TEST(TranslationRecovery, ZeroRelativeTranslations) { - // Create a graph with 3 cameras. +TEST(TranslationRecovery, TwoPointTest) { + // Create a dataset with 2 poses. // __ __ // \/ \/ // 0 _____ 1 - // 2 <| // - // 0 and 1 face in the same direction but have a translation offset. 2 is at - // the same point as 1 but is rotated, with very little FOV overlap. + // 0 and 1 face in the same direction but have a translation offset. Values poses; - poses.insert(0, Pose3(Rot3(), Point3())); - poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); - poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); auto relativeTranslations = - TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}}); + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}}); - // Check + // Check simulated measurements. for (auto& unitTranslation : relativeTranslations) { - const Pose3 wTa = poses.at(unitTranslation.key1()), - wTb = poses.at(unitTranslation.key2()); - const Point3 Ta = wTa.translation(), Tb = wTb.translation(); - const Unit3 w_aZb = unitTranslation.measured(); - EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); } TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); EXPECT_LONGS_EQUAL(1, graph.size()); - // Translation recovery, version 1 - const double scale = 2.0; - const auto result = algorithm.run(scale); + // Run translation recovery + const auto result = algorithm.run(/*scale=*/2.0); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); EXPECT(assert_equal(Point3(2, 0, 0), result.at(1))); +} + +TEST(TranslationRecovery, ThreePointTest) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 3}, {3, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(3, graph.size()); + + const auto result = algorithm.run(/*scale=*/2.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3))); +} + +TEST(TranslationRecovery, TwoPointsAndZeroTranslation) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // 2 <| + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with little FOV overlap. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + // There is only 1 non-zero translation edge. + EXPECT_LONGS_EQUAL(1, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(/*scale=*/2.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1))); EXPECT(assert_equal(Point3(2, 0, 0), result.at(2))); } -TEST(TranslationRecovery, ZeroRelativeTranslations4Cameras) { - // Create a graph with 4 cameras. +TEST(TranslationRecovery, ThreePointsAndZeroTranslation) { + // Create a dataset with 4 poses. // __ __ // \/ \/ // 0 _____ 1 - // \ 2 <| - // \ / + // \ __ 2 <| + // \\// // 3 // // 0 and 1 face in the same direction but have a translation offset. 2 is at @@ -137,32 +210,28 @@ TEST(TranslationRecovery, ZeroRelativeTranslations4Cameras) { // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. Values poses; - poses.insert(0, Pose3(Rot3(), Point3())); - poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); - poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); - poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); auto relativeTranslations = TranslationRecovery::SimulateMeasurements( poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}}); - // Check + // Check simulated measurements. for (auto& unitTranslation : relativeTranslations) { - const Pose3 wTa = poses.at(unitTranslation.key1()), - wTb = poses.at(unitTranslation.key2()); - const Point3 Ta = wTa.translation(), Tb = wTb.translation(); - const Unit3 w_aZb = unitTranslation.measured(); - EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); } TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); EXPECT_LONGS_EQUAL(3, graph.size()); - // Translation recovery, version 1 - const double scale = 2.0; - const auto result = algorithm.run(scale); + // Run translation recovery + const auto result = algorithm.run(/*scale=*/2.0); - // Check result for first two translations, determined by prior + // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); EXPECT(assert_equal(Point3(2, 0, 0), result.at(1))); EXPECT(assert_equal(Point3(2, 0, 0), result.at(2))); From 602db46f449fc2f9a2629dcce291f4e9a74edd2c Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Tue, 1 Dec 2020 01:33:43 -0800 Subject: [PATCH 3/4] changing test names and adding documentation --- gtsam/sfm/TranslationRecovery.cpp | 20 ++++++++++++++----- tests/testTranslationRecovery.cpp | 32 +++++++++++++++---------------- 2 files changed, 31 insertions(+), 21 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 8d27136e3..d4100b00a 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -39,9 +39,13 @@ TranslationRecovery::TranslationRecovery( const TranslationRecovery::TranslationEdges &relativeTranslations, const LevenbergMarquardtParams &lmParams) : params_(lmParams) { - TranslationEdges tempRelativeTranslations; - DSFMap sameTranslationDSF; + // Some relative translations may be zero. We treat nodes that have a zero + // relativeTranslation as a single node. + // A DSFMap is used to find sets of nodes that have a zero relative + // translation. Add the nodes in each edge to the DSFMap, and merge nodes that + // are connected by a zero relative translation. + DSFMap sameTranslationDSF; for (const auto &edge : relativeTranslations) { Key key1 = sameTranslationDSF.find(edge.key1()); Key key2 = sameTranslationDSF.find(edge.key2()); @@ -49,6 +53,7 @@ TranslationRecovery::TranslationRecovery( sameTranslationDSF.merge(key1, key2); } } + // Use only those edges for which two keys have a distinct root in the DSFMap. for (const auto &edge : relativeTranslations) { Key key1 = sameTranslationDSF.find(edge.key1()); Key key2 = sameTranslationDSF.find(edge.key2()); @@ -56,6 +61,7 @@ TranslationRecovery::TranslationRecovery( relativeTranslations_.emplace_back(key1, key2, edge.measured(), edge.noiseModel()); } + // Store the DSF map for post-processing results. sameTranslationNodes_ = sameTranslationDSF.sets(); } @@ -106,9 +112,13 @@ Values TranslationRecovery::run(const double scale) const { LevenbergMarquardtOptimizer lm(graph, initial, params_); Values result = lm.optimize(); - for (const auto &sameTranslationKeys : sameTranslationNodes_) { - Key optimizedKey = sameTranslationKeys.first; - std::set duplicateKeys = sameTranslationKeys.second; + // Nodes that were not optimized are stored in sameTranslationNodes_ as a map + // from a key that was optimized to keys that were not optimized. Iterate over + // map and add results for keys not optimized. + for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { + Key optimizedKey = optimizedAndDuplicateKeys.first; + std::set duplicateKeys = optimizedAndDuplicateKeys.second; + // Add the result for the duplicate key if it does not already exist. for (const Key duplicateKey : duplicateKeys) { if (result.exists(duplicateKey)) continue; result.insert(duplicateKey, result.at(optimizedKey)); diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index e4fbd9219..7260fd5af 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -89,7 +89,7 @@ TEST(TranslationRecovery, BAL) { // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); } -TEST(TranslationRecovery, TwoPointTest) { +TEST(TranslationRecovery, TwoPoseTest) { // Create a dataset with 2 poses. // __ __ // \/ \/ @@ -114,14 +114,14 @@ TEST(TranslationRecovery, TwoPointTest) { EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/2.0); + 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(2, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); } -TEST(TranslationRecovery, ThreePointTest) { +TEST(TranslationRecovery, ThreePoseTest) { // Create a dataset with 3 poses. // __ __ // \/ \/ @@ -151,15 +151,15 @@ TEST(TranslationRecovery, ThreePointTest) { const auto graph = algorithm.buildGraph(); EXPECT_LONGS_EQUAL(3, graph.size()); - const auto result = algorithm.run(/*scale=*/2.0); + const auto result = algorithm.run(/*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(2, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(1, -1, 0), result.at(3))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at(3))); } -TEST(TranslationRecovery, TwoPointsAndZeroTranslation) { +TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { // Create a dataset with 3 poses. // __ __ // \/ \/ @@ -188,15 +188,15 @@ TEST(TranslationRecovery, TwoPointsAndZeroTranslation) { EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/2.0); + const auto result = algorithm.run(/*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(2, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(2, 0, 0), result.at(2))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(2))); } -TEST(TranslationRecovery, ThreePointsAndZeroTranslation) { +TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { // Create a dataset with 4 poses. // __ __ // \/ \/ @@ -229,13 +229,13 @@ TEST(TranslationRecovery, ThreePointsAndZeroTranslation) { EXPECT_LONGS_EQUAL(3, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/2.0); + const auto result = algorithm.run(/*scale=*/4.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(2, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(2, 0, 0), result.at(2))); - EXPECT(assert_equal(Point3(1, -1, 0), result.at(3))); + 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))); } /* ************************************************************************* */ From 6d2e306aa8781eedf4af470259609858e480e2a0 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Tue, 1 Dec 2020 09:10:32 -0800 Subject: [PATCH 4/4] documenting member variables --- gtsam/sfm/TranslationRecovery.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 9ffe45685..c99836853 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -54,8 +54,14 @@ class TranslationRecovery { using TranslationEdges = std::vector>; private: + // Translation directions between camera pairs. TranslationEdges relativeTranslations_; + + // Parameters used by the LM Optimizer. LevenbergMarquardtParams params_; + + // Map from a key in the graph to a set of keys that share the same + // translation. std::map> sameTranslationNodes_; public: