From 4bc250e7c04c4b91a3f15715d0e70dd82ede388b Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sat, 28 Nov 2020 23:44:20 -0800 Subject: [PATCH] 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;