diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index da258bbc0..ce332ed0b 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -79,28 +79,29 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { graph.emplace_shared(edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); } - - // Add between factors for optional relative translations. - for (auto edge : params_.getBetweenTranslations()) { - Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2()); - if (k1 != k2) { - graph.emplace_shared>(k1, k2, edge.measured(), - edge.noiseModel()); - } - } - return graph; } void TranslationRecovery::addPrior( + const std::vector> &betweenTranslations, const double scale, const boost::shared_ptr graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); if (edge == relativeTranslations_.end()) return; graph->emplace_shared>(edge->key1(), Point3(0, 0, 0), priorNoiseModel); + + // Add between factors for optional relative translations. + for (auto edge : betweenTranslations) { + Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2()); + if (k1 != k2) { + graph->emplace_shared>(k1, k2, edge.measured(), + edge.noiseModel()); + } + } + // Add a scale prior only if no other between factors were added. - if (params_.getBetweenTranslations().empty()) { + if (betweenTranslations.empty()) { graph->emplace_shared>( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); } @@ -154,10 +155,12 @@ Values TranslationRecovery::initializeRandomly() const { return initializeRandomly(&kRandomNumberGenerator); } -Values TranslationRecovery::run(const double scale) const { +Values TranslationRecovery::run( + const std::vector> &betweenTranslations, + const double scale) const { boost::shared_ptr graph_ptr = boost::make_shared(buildGraph()); - addPrior(scale, graph_ptr); + addPrior(betweenTranslations, scale, graph_ptr); const Values initial = initializeRandomly(); LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams()); Values result = lm.optimize(); diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index e555941e0..33de91bf1 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -31,15 +31,6 @@ namespace gtsam { // Parameters for the Translation Recovery problem. class TranslationRecoveryParams { public: - std::vector> getBetweenTranslations() const { - return betweenTranslations_; - } - - void setBetweenTranslations( - const std::vector> &betweenTranslations) { - betweenTranslations_ = betweenTranslations; - } - LevenbergMarquardtParams getLMParams() const { return lmParams_; } Values getInitialValues() const { return initial_; } @@ -51,10 +42,6 @@ class TranslationRecoveryParams { } private: - // Relative translations with a known scale used as between factors in the - // problem if provided. - std::vector> betweenTranslations_; - // LevenbergMarquardtParams for optimization. LevenbergMarquardtParams lmParams_; @@ -125,10 +112,11 @@ class TranslationRecovery { * @param graph factor graph to which prior is added. * @param priorNoiseModel the noise model to use with the prior. */ - void addPrior(const double scale, - const boost::shared_ptr graph, - const SharedNoiseModel &priorNoiseModel = - noiseModel::Isotropic::Sigma(3, 0.01)) const; + void addPrior( + const std::vector> &betweenTranslations, + const double scale, const boost::shared_ptr graph, + const SharedNoiseModel &priorNoiseModel = + noiseModel::Isotropic::Sigma(3, 0.01)) const; /** * @brief Create random initial translations. @@ -152,7 +140,9 @@ class TranslationRecovery { * The scale is only used if relativeTranslations in the params is empty. * @return Values */ - Values run(const double scale = 1.0) const; + Values run( + const std::vector> &betweenTranslations = {}, + const double scale = 1.0) const; /** * @brief Simulate translation direction measurements diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index b419d8c58..6c7233a37 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -68,7 +68,7 @@ TEST(TranslationRecovery, BAL) { // Run translation recovery const double scale = 2.0; - const auto result = algorithm.run(scale); + const auto result = algorithm.run(/*betweenTranslations=*/{}, scale); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); @@ -112,7 +112,7 @@ TEST(TranslationRecovery, TwoPoseTest) { EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/3.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -149,7 +149,7 @@ TEST(TranslationRecovery, ThreePoseTest) { const auto graph = algorithm.buildGraph(); EXPECT_LONGS_EQUAL(3, graph.size()); - const auto result = algorithm.run(/*scale=*/3.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -186,7 +186,7 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/3.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -227,7 +227,7 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { EXPECT_LONGS_EQUAL(3, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/4.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -257,7 +257,7 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { EXPECT_LONGS_EQUAL(0, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/4.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -282,16 +282,15 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) { 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}, {0, 3}, {1, 3}}); + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); - TranslationRecoveryParams params; std::vector> betweenTranslations; - betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), noiseModel::Isotropic::Sigma(3, 1e-2)); - params.setBetweenTranslations(betweenTranslations); + betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), + noiseModel::Isotropic::Sigma(3, 1e-2)); - TranslationRecovery algorithm(relativeTranslations, params); - auto result = algorithm.run(); + TranslationRecovery algorithm(relativeTranslations); + auto result = algorithm.run(betweenTranslations); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); @@ -299,7 +298,6 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) { EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); } - TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { // Create a dataset with 3 poses. // __ __ @@ -317,16 +315,15 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { 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}, {0, 3}, {1, 3}}); + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); - TranslationRecoveryParams params; std::vector> betweenTranslations; - betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), noiseModel::Constrained::All(3, 1e2)); - params.setBetweenTranslations(betweenTranslations); + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); - TranslationRecovery algorithm(relativeTranslations, params); - auto result = algorithm.run(); + TranslationRecovery algorithm(relativeTranslations); + auto result = algorithm.run(betweenTranslations); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4));