From 3ea19f4bd0df147558c46a2c82ee8c2928a580f0 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 23 Apr 2022 19:08:16 +0530 Subject: [PATCH 01/13] add additional methods to TA + wrapper --- gtsam/sfm/TranslationRecovery.cpp | 48 ++++++++++++++++++++++++++----- gtsam/sfm/TranslationRecovery.h | 17 ++++++++++- gtsam/sfm/sfm.i | 16 +++++++++-- tests/testTranslationRecovery.cpp | 39 ++++++++++++++++++++++++- 4 files changed, 109 insertions(+), 11 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 2e81c2d561..7938d6b936 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include #include #include @@ -81,7 +83,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { } void TranslationRecovery::addPrior( - const double scale, NonlinearFactorGraph *graph, + const double scale, const boost::shared_ptr graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); if (edge == relativeTranslations_.end()) return; @@ -91,6 +93,32 @@ void TranslationRecovery::addPrior( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); } +void TranslationRecovery::addPrior( + Key i, const Point3 &prior, + const boost::shared_ptr graph, + const SharedNoiseModel &priorNoiseModel) const { + graph->addPrior(getUniqueKey(i), prior, priorNoiseModel); +} + +Key TranslationRecovery::getUniqueKey(const Key i) const { + for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { + Key optimizedKey = optimizedAndDuplicateKeys.first; + std::set duplicateKeys = optimizedAndDuplicateKeys.second; + if (i == optimizedKey || duplicateKeys.count(i)) return optimizedKey; + } + // Unlikely case, when i is not in the graph. + return i; +} + +void TranslationRecovery::addRelativeHardConstraint( + Key i, Key j, const Point3 &w_itj, + const boost::shared_ptr graph) const { + Point3_ wti_(getUniqueKey(i)), wtj_(getUniqueKey(j)); + Expression w_itj_ = wtj_ - wti_; + graph->addExpressionFactor(noiseModel::Constrained::All(3, 1e9), w_itj, + w_itj_); +} + Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly @@ -125,12 +153,17 @@ Values TranslationRecovery::initializeRandomly() const { } Values TranslationRecovery::run(const double scale) const { - auto graph = buildGraph(); - addPrior(scale, &graph); + boost::shared_ptr graph_ptr = + boost::make_shared(buildGraph()); + addPrior(scale, graph_ptr); const Values initial = initializeRandomly(); - LevenbergMarquardtOptimizer lm(graph, initial, params_); + LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_); Values result = lm.optimize(); + return addDuplicateNodes(result); +} +Values TranslationRecovery::addDuplicateNodes(const Values &result) const { + Values final_result = result; // 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. @@ -139,11 +172,12 @@ Values TranslationRecovery::run(const double scale) const { 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)); + if (final_result.exists(duplicateKey)) continue; + final_result.insert(duplicateKey, + final_result.at(optimizedKey)); } } - return result; + return final_result; } TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 30c9a14e39..a0bcb5dd46 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -93,10 +93,20 @@ 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, NonlinearFactorGraph *graph, + void addPrior(const double scale, + const boost::shared_ptr graph, const SharedNoiseModel &priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01)) const; + void addPrior(Key i, const Point3 &prior, + const boost::shared_ptr graph, + const SharedNoiseModel &priorNoiseModel = + noiseModel::Isotropic::Sigma(3, 0.01)) const; + + void addRelativeHardConstraint( + Key i, Key j, const Point3 &w_itj, + const boost::shared_ptr graph) const; + /** * @brief Create random initial translations. * @@ -120,6 +130,8 @@ class TranslationRecovery { */ Values run(const double scale = 1.0) const; + Values addDuplicateNodes(const Values &result) const; + /** * @brief Simulate translation direction measurements * @@ -131,5 +143,8 @@ class TranslationRecovery { */ static TranslationEdges SimulateMeasurements( const Values &poses, const std::vector &edges); + + private: + Key getUniqueKey(const Key i) const; }; } // namespace gtsam diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index bf9a73ac53..36a73b7924 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,6 +4,8 @@ namespace gtsam { +#include +#include #include class SfmTrack { SfmTrack(); @@ -142,8 +144,8 @@ class ShonanAveraging2 { ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile, const gtsam::ShonanAveragingParameters2& parameters); - ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, - const gtsam::ShonanAveragingParameters2 ¶meters); + ShonanAveraging2(const gtsam::BetweenFactorPose2s& factors, + const gtsam::ShonanAveragingParameters2& parameters); // Query properties size_t nrUnknowns() const; @@ -259,6 +261,16 @@ class TranslationRecovery { TranslationRecovery( const gtsam::BinaryMeasurementsUnit3& relativeTranslations); // default LevenbergMarquardtParams + gtsam::NonlinearFactorGraph buildGraph() const; + gtsam::Values initializeRandomly() const; + void addPrior(gtsam::Key i, const gtsam::Point3& prior, + gtsam::NonlinearFactorGraph* graph, + const gtsam::SharedNoiseModel& model = + gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const; + void addRelativeHardConstraint(gtsam::Key i, gtsam::Key j, + const gtsam::Point3& w_itj, + gtsam::NonlinearFactorGraph* graph) const; + gtsam::Values addDuplicateNodes(const gtsam::Values& result) const; gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 833f11355d..006750252f 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -17,8 +17,8 @@ */ #include -#include #include +#include #include using namespace std; @@ -265,6 +265,43 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { EXPECT(assert_equal(Point3(0, 0, 0), result.at(2), 1e-8)); } +TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { + // 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}, {3, 0}}); + + TranslationRecovery algorithm(relativeTranslations); + boost::shared_ptr graph_ptr = + boost::make_shared(algorithm.buildGraph()); + algorithm.addPrior(0, Point3(), graph_ptr); + algorithm.addRelativeHardConstraint(0, 1, Point3(2, 0, 0), graph_ptr); + const Values initial = algorithm.initializeRandomly(); + LevenbergMarquardtParams params; + LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params); + auto result = algorithm.addDuplicateNodes(lm.optimize()); + EXPECT_LONGS_EQUAL(4, graph_ptr->size()); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 1d6fd5409af322749b84b07c90986d99e6a88ee8 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 5 May 2022 18:40:58 -0700 Subject: [PATCH 02/13] change to input params, add docstring --- gtsam/sfm/TranslationRecovery.cpp | 57 ++++++++++++------------ gtsam/sfm/TranslationRecovery.h | 73 ++++++++++++++++++++++++------- tests/testTranslationRecovery.cpp | 55 ++++++++++++++++++----- 3 files changed, 129 insertions(+), 56 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 7938d6b936..da258bbc0b 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -42,8 +43,8 @@ static std::mt19937 kRandomNumberGenerator(42); TranslationRecovery::TranslationRecovery( const TranslationRecovery::TranslationEdges &relativeTranslations, - const LevenbergMarquardtParams &lmParams) - : params_(lmParams) { + const TranslationRecoveryParams ¶ms) + : params_(params) { // Some relative translations may be zero. We treat nodes that have a zero // relativeTranslation as a single node. @@ -73,12 +74,21 @@ TranslationRecovery::TranslationRecovery( NonlinearFactorGraph TranslationRecovery::buildGraph() const { NonlinearFactorGraph graph; - // Add all relative translation edges + // Add translation factors for input translation directions. for (auto edge : relativeTranslations_) { 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; } @@ -87,17 +97,13 @@ void TranslationRecovery::addPrior( 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 >( - edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); -} - -void TranslationRecovery::addPrior( - Key i, const Point3 &prior, - const boost::shared_ptr graph, - const SharedNoiseModel &priorNoiseModel) const { - graph->addPrior(getUniqueKey(i), prior, priorNoiseModel); + graph->emplace_shared>(edge->key1(), Point3(0, 0, 0), + priorNoiseModel); + // Add a scale prior only if no other between factors were added. + if (params_.getBetweenTranslations().empty()) { + graph->emplace_shared>( + edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); + } } Key TranslationRecovery::getUniqueKey(const Key i) const { @@ -110,25 +116,21 @@ Key TranslationRecovery::getUniqueKey(const Key i) const { return i; } -void TranslationRecovery::addRelativeHardConstraint( - Key i, Key j, const Point3 &w_itj, - const boost::shared_ptr graph) const { - Point3_ wti_(getUniqueKey(i)), wtj_(getUniqueKey(j)); - Expression w_itj_ = wtj_ - wti_; - graph->addExpressionFactor(noiseModel::Constrained::All(3, 1e9), w_itj, - w_itj_); -} - 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; + const Values inputInitial = params_.getInitialValues(); auto insert = [&](Key j) { - if (!initial.exists(j)) { + if (initial.exists(j)) return; + if (inputInitial.exists(j)) { + initial.insert(j, inputInitial.at(j)); + } else { initial.insert( j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); } + // Assumes all nodes connected by zero-edges have the same initialization. }; // Loop over measurements and add a random translation @@ -157,12 +159,13 @@ Values TranslationRecovery::run(const double scale) const { boost::make_shared(buildGraph()); addPrior(scale, graph_ptr); const Values initial = initializeRandomly(); - LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_); + LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams()); Values result = lm.optimize(); - return addDuplicateNodes(result); + return addSameTranslationNodes(result); } -Values TranslationRecovery::addDuplicateNodes(const Values &result) const { +Values TranslationRecovery::addSameTranslationNodes( + const Values &result) const { Values final_result = result; // 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 diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index a0bcb5dd46..e555941e03 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -28,6 +28,40 @@ 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_; } + + void setInitialValues(const Values &values) { initial_ = values; } + + void setLMParams(const LevenbergMarquardtParams &lmParams) { + lmParams_ = lmParams; + } + + private: + // Relative translations with a known scale used as between factors in the + // problem if provided. + std::vector> betweenTranslations_; + + // LevenbergMarquardtParams for optimization. + LevenbergMarquardtParams lmParams_; + + // Initial values, random intialization will be used if not provided. + Values initial_; +}; + // Set up an optimization problem for the unknown translations Ti in the world // coordinate frame, given the known camera attitudes wRi with respect to the // world frame, and a set of (noisy) translation directions of type Unit3, @@ -57,8 +91,8 @@ class TranslationRecovery { // Translation directions between camera pairs. TranslationEdges relativeTranslations_; - // Parameters used by the LM Optimizer. - LevenbergMarquardtParams params_; + // Parameters. + TranslationRecoveryParams params_; // Map from a key in the graph to a set of keys that share the same // translation. @@ -71,13 +105,11 @@ class TranslationRecovery { * @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. - * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be - * used to modify the parameters for the LM optimizer. By default, uses the - * default LM parameters. + * @param params (optional) parameters for the recovery problem. */ TranslationRecovery( const TranslationEdges &relativeTranslations, - const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()); + const TranslationRecoveryParams ¶ms = TranslationRecoveryParams()); /** * @brief Build the factor graph to do the optimization. @@ -98,15 +130,6 @@ class TranslationRecovery { const SharedNoiseModel &priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01)) const; - void addPrior(Key i, const Point3 &prior, - const boost::shared_ptr graph, - const SharedNoiseModel &priorNoiseModel = - noiseModel::Isotropic::Sigma(3, 0.01)) const; - - void addRelativeHardConstraint( - Key i, Key j, const Point3 &w_itj, - const boost::shared_ptr graph) const; - /** * @brief Create random initial translations. * @@ -126,12 +149,11 @@ class TranslationRecovery { * @brief Build and optimize factor graph. * * @param scale scale for first relative translation which fixes gauge. + * The scale is only used if relativeTranslations in the params is empty. * @return Values */ Values run(const double scale = 1.0) const; - Values addDuplicateNodes(const Values &result) const; - /** * @brief Simulate translation direction measurements * @@ -145,6 +167,23 @@ class TranslationRecovery { const Values &poses, const std::vector &edges); private: + /** + * @brief Gets the key of the variable being optimized among multiple input + * variables that have the same translation. + * + * @param i key of input variable. + * @return Key of optimized variable - same as input if it does not have any + * zero-translation edges. + */ Key getUniqueKey(const Key i) const; + + /** + * @brief Adds nodes that were not optimized for because they were connected + * to another node with a zero-translation edge in the input. + * + * @param result optimization problem result + * @return translation estimates for all variables in the input. + */ + Values addSameTranslationNodes(const Values &result) const; }; } // namespace gtsam diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 006750252f..b419d8c585 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -265,7 +265,7 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { EXPECT(assert_equal(Point3(0, 0, 0), result.at(2), 1e-8)); } -TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { +TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) { // Create a dataset with 3 poses. // __ __ // \/ \/ @@ -283,18 +283,15 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); auto relativeTranslations = - TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {3, 0}}); + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {0, 3}, {1, 3}}); - TranslationRecovery algorithm(relativeTranslations); - boost::shared_ptr graph_ptr = - boost::make_shared(algorithm.buildGraph()); - algorithm.addPrior(0, Point3(), graph_ptr); - algorithm.addRelativeHardConstraint(0, 1, Point3(2, 0, 0), graph_ptr); - const Values initial = algorithm.initializeRandomly(); - LevenbergMarquardtParams params; - LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params); - auto result = algorithm.addDuplicateNodes(lm.optimize()); - EXPECT_LONGS_EQUAL(4, graph_ptr->size()); + TranslationRecoveryParams params; + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), noiseModel::Isotropic::Sigma(3, 1e-2)); + params.setBetweenTranslations(betweenTranslations); + + TranslationRecovery algorithm(relativeTranslations, params); + auto result = algorithm.run(); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); @@ -302,6 +299,40 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); } + +TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { + // 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}, {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); + + TranslationRecovery algorithm(relativeTranslations, params); + auto result = algorithm.run(); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); +} /* ************************************************************************* */ int main() { TestResult tr; From 279b9bedf9aed461e19422d2f0dac8381af4143c Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 5 May 2022 18:52:01 -0700 Subject: [PATCH 03/13] wrapper changes --- gtsam/sfm/sfm.i | 36 ++++++++++++++++++------------ gtsam/slam/dataset.h | 1 + python/CMakeLists.txt | 2 ++ python/gtsam/specializations/sfm.h | 2 ++ 4 files changed, 27 insertions(+), 14 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 36a73b7924..2b6f37a45b 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -85,6 +85,7 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +typedef gtsam::BinaryMeasurement BinaryMeasurementPoint3; class BinaryMeasurementsUnit3 { BinaryMeasurementsUnit3(); @@ -93,6 +94,13 @@ class BinaryMeasurementsUnit3 { void push_back(const gtsam::BinaryMeasurement& measurement); }; +class BinaryMeasurementsPoint3 { + BinaryMeasurementsPoint3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in @@ -254,23 +262,23 @@ class MFAS { }; #include +class TranslationRecoveryParams { + gtsam::BinaryMeasurementsPoint3 getBetweenTranslations() const; + gtsam::Values getInitialValues() const; + gtsam::LevenbergMarquardtParams getLMParams() const; + + void setBetweenTranslations( + const gtsam::BinaryMeasurementsPoint3& betweenTranslations); + void setInitialValues(const gtsam::Values& values); + void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams); +}; + class TranslationRecovery { TranslationRecovery( const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::LevenbergMarquardtParams& lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3& - relativeTranslations); // default LevenbergMarquardtParams - gtsam::NonlinearFactorGraph buildGraph() const; - gtsam::Values initializeRandomly() const; - void addPrior(gtsam::Key i, const gtsam::Point3& prior, - gtsam::NonlinearFactorGraph* graph, - const gtsam::SharedNoiseModel& model = - gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const; - void addRelativeHardConstraint(gtsam::Key i, gtsam::Key j, - const gtsam::Point3& w_itj, - gtsam::NonlinearFactorGraph* graph) const; - gtsam::Values addDuplicateNodes(const gtsam::Values& result) const; + const gtsam::TranslationRecoveryParams& lmParams); + TranslationRecovery(const gtsam::BinaryMeasurementsUnit3& + relativeTranslations); // default params gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index dc450a9f77..247be5bae1 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename, size_t maxIndex = 0); using BinaryMeasurementsUnit3 = std::vector>; +using BinaryMeasurementsPoint3 = std::vector>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 inline boost::optional GTSAM_DEPRECATED diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f5869b1450..6c0c6e83a7 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -47,6 +47,7 @@ set(ignore gtsam::Pose3Vector gtsam::Rot3Vector gtsam::KeyVector + gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::DiscreteKey gtsam::KeyPairDoubleMap) @@ -124,6 +125,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::Pose3Vector gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue + gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 6de15217fb..7a5084d4a8 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -11,6 +11,8 @@ * and saves one copy operation. */ +py::bind_vector > >( + m_, "BinaryMeasurementsPoint3"); py::bind_vector > >( m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "KeyPairDoubleMap"); From e517c344640882fb3e68d97392228abe3e5a925f Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 09:12:56 -0700 Subject: [PATCH 04/13] move betweenTranslations outside params --- gtsam/sfm/TranslationRecovery.cpp | 29 ++++++++++++----------- gtsam/sfm/TranslationRecovery.h | 26 +++++++-------------- tests/testTranslationRecovery.cpp | 39 ++++++++++++++----------------- 3 files changed, 42 insertions(+), 52 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index da258bbc0b..ce332ed0be 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 e555941e03..33de91bf1a 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 b419d8c585..6c7233a37e 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)); From 0e8c5eb5a7bd93265199add2083ce81091ec43c6 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 09:23:00 -0700 Subject: [PATCH 05/13] ../gtsfm/sfm/sfm.i --- gtsam/sfm/sfm.i | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 2b6f37a45b..064bc1e635 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -263,12 +263,9 @@ class MFAS { #include class TranslationRecoveryParams { - gtsam::BinaryMeasurementsPoint3 getBetweenTranslations() const; gtsam::Values getInitialValues() const; gtsam::LevenbergMarquardtParams getLMParams() const; - void setBetweenTranslations( - const gtsam::BinaryMeasurementsPoint3& betweenTranslations); void setInitialValues(const gtsam::Values& values); void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams); }; @@ -279,6 +276,9 @@ class TranslationRecovery { const gtsam::TranslationRecoveryParams& lmParams); TranslationRecovery(const gtsam::BinaryMeasurementsUnit3& relativeTranslations); // default params + gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + const double scale) const; + // default empty betweenTranslations gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; From b42d3a3d2ff9bf50f98e07ba9d6df6aff5782ed2 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 11:24:31 -0700 Subject: [PATCH 06/13] change params to struct --- gtsam/sfm/TranslationRecovery.cpp | 14 ++++++-------- gtsam/sfm/TranslationRecovery.h | 20 ++++---------------- gtsam/sfm/sfm.i | 9 +++------ 3 files changed, 13 insertions(+), 30 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index ce332ed0be..ce8e9c0790 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -84,7 +84,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { void TranslationRecovery::addPrior( const std::vector> &betweenTranslations, - const double scale, const boost::shared_ptr graph, + const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); if (edge == relativeTranslations_.end()) return; @@ -122,11 +122,10 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { // Create a lambda expression that checks whether value exists and randomly // initializes if not. Values initial; - const Values inputInitial = params_.getInitialValues(); auto insert = [&](Key j) { if (initial.exists(j)) return; - if (inputInitial.exists(j)) { - initial.insert(j, inputInitial.at(j)); + if (params_.initial.exists(j)) { + initial.insert(j, params_.initial.at(j)); } else { initial.insert( j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); @@ -158,11 +157,10 @@ Values TranslationRecovery::initializeRandomly() const { Values TranslationRecovery::run( const std::vector> &betweenTranslations, const double scale) const { - boost::shared_ptr graph_ptr = - boost::make_shared(buildGraph()); - addPrior(betweenTranslations, scale, graph_ptr); + NonlinearFactorGraph graph = buildGraph(); + addPrior(betweenTranslations, scale, &graph); const Values initial = initializeRandomly(); - LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams()); + LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); Values result = lm.optimize(); return addSameTranslationNodes(result); } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 33de91bf1a..0b25ad7b4e 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -29,24 +29,12 @@ namespace gtsam { // Parameters for the Translation Recovery problem. -class TranslationRecoveryParams { - public: - LevenbergMarquardtParams getLMParams() const { return lmParams_; } - - Values getInitialValues() const { return initial_; } - - void setInitialValues(const Values &values) { initial_ = values; } - - void setLMParams(const LevenbergMarquardtParams &lmParams) { - lmParams_ = lmParams; - } - - private: +struct TranslationRecoveryParams { // LevenbergMarquardtParams for optimization. - LevenbergMarquardtParams lmParams_; + LevenbergMarquardtParams lmParams; // Initial values, random intialization will be used if not provided. - Values initial_; + Values initial; }; // Set up an optimization problem for the unknown translations Ti in the world @@ -114,7 +102,7 @@ class TranslationRecovery { */ void addPrior( const std::vector> &betweenTranslations, - const double scale, const boost::shared_ptr graph, + const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01)) const; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 064bc1e635..66280037c2 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -263,11 +263,8 @@ class MFAS { #include class TranslationRecoveryParams { - gtsam::Values getInitialValues() const; - gtsam::LevenbergMarquardtParams getLMParams() const; - - void setInitialValues(const gtsam::Values& values); - void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams); + gtsam::LevenbergMarquardtParams lmParams; + gtsam::Values initial; }; class TranslationRecovery { @@ -279,7 +276,7 @@ class TranslationRecovery { gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations, const double scale) const; // default empty betweenTranslations - gtsam::Values run(const double scale) const; + // gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; From c1a7cf21d58e2bb2e42db1838c492aa854a69b2e Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 11:26:21 -0700 Subject: [PATCH 07/13] rename getUniqueKey --- gtsam/sfm/TranslationRecovery.cpp | 5 +++-- gtsam/sfm/TranslationRecovery.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index ce8e9c0790..aa75b8fc6b 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -93,7 +93,8 @@ void TranslationRecovery::addPrior( // Add between factors for optional relative translations. for (auto edge : betweenTranslations) { - Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2()); + Key k1 = getSameTranslationRootNode(edge.key1()), + k2 = getSameTranslationRootNode(edge.key2()); if (k1 != k2) { graph->emplace_shared>(k1, k2, edge.measured(), edge.noiseModel()); @@ -107,7 +108,7 @@ void TranslationRecovery::addPrior( } } -Key TranslationRecovery::getUniqueKey(const Key i) const { +Key TranslationRecovery::getSameTranslationRootNode(const Key i) const { for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { Key optimizedKey = optimizedAndDuplicateKeys.first; std::set duplicateKeys = optimizedAndDuplicateKeys.second; diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 0b25ad7b4e..931d072c81 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -153,7 +153,7 @@ class TranslationRecovery { * @return Key of optimized variable - same as input if it does not have any * zero-translation edges. */ - Key getUniqueKey(const Key i) const; + Key getSameTranslationRootNode(const Key i) const; /** * @brief Adds nodes that were not optimized for because they were connected From 230bb8eb115254739faa6bd6609b8d6434d2b016 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 18:15:44 -0700 Subject: [PATCH 08/13] move relativeTranslations to run() --- gtsam/sfm/TranslationRecovery.cpp | 165 ++++++++++++++++-------------- gtsam/sfm/TranslationRecovery.h | 65 +++++------- tests/testTranslationRecovery.cpp | 49 ++++----- 3 files changed, 137 insertions(+), 142 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index aa75b8fc6b..122b17ce68 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -41,16 +41,13 @@ 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 TranslationRecoveryParams ¶ms) - : params_(params) { - // 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. +// 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 getSameTranslationDSFMap( + const std::vector> &relativeTranslations) { DSFMap sameTranslationDSF; for (const auto &edge : relativeTranslations) { Key key1 = sameTranslationDSF.find(edge.key1()); @@ -59,23 +56,52 @@ 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()); + return sameTranslationDSF; +} + +// Removes zero-translation edges from measurements, and combines the nodes in +// these edges into a single node. +template +std::vector> removeSameTranslationNodes( + const std::vector> &edges, + const DSFMap &sameTranslationDSFMap) { + std::vector> newEdges; + for (const auto &edge : edges) { + Key key1 = sameTranslationDSFMap.find(edge.key1()); + Key key2 = sameTranslationDSFMap.find(edge.key2()); if (key1 == key2) continue; - relativeTranslations_.emplace_back(key1, key2, edge.measured(), - edge.noiseModel()); + newEdges.emplace_back(key1, key2, edge.measured(), edge.noiseModel()); + } + return newEdges; +} + +// Adds nodes that were not optimized for because they were connected +// to another node with a zero-translation edge in the input. +Values addSameTranslationNodes(const Values &result, + const DSFMap &sameTranslationDSFMap) { + Values final_result = result; + // 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 : sameTranslationDSFMap.sets()) { + 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 (final_result.exists(duplicateKey)) continue; + final_result.insert(duplicateKey, + final_result.at(optimizedKey)); + } } - // Store the DSF map for post-processing results. - sameTranslationNodes_ = sameTranslationDSF.sets(); + return final_result; } -NonlinearFactorGraph TranslationRecovery::buildGraph() const { +NonlinearFactorGraph TranslationRecovery::buildGraph( + const std::vector> &relativeTranslations) const { NonlinearFactorGraph graph; // Add translation factors for input translation directions. - for (auto edge : relativeTranslations_) { + for (auto edge : relativeTranslations) { graph.emplace_shared(edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); } @@ -83,22 +109,20 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { } void TranslationRecovery::addPrior( + const std::vector> &relativeTranslations, + const double scale, const std::vector> &betweenTranslations, - const double scale, NonlinearFactorGraph *graph, + NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel) const { - auto edge = relativeTranslations_.begin(); - if (edge == relativeTranslations_.end()) return; + 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 = getSameTranslationRootNode(edge.key1()), - k2 = getSameTranslationRootNode(edge.key2()); - if (k1 != k2) { - graph->emplace_shared>(k1, k2, edge.measured(), - edge.noiseModel()); - } + graph->emplace_shared>( + edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); } // Add a scale prior only if no other between factors were added. @@ -108,17 +132,9 @@ void TranslationRecovery::addPrior( } } -Key TranslationRecovery::getSameTranslationRootNode(const Key i) const { - for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { - Key optimizedKey = optimizedAndDuplicateKeys.first; - std::set duplicateKeys = optimizedAndDuplicateKeys.second; - if (i == optimizedKey || duplicateKeys.count(i)) return optimizedKey; - } - // Unlikely case, when i is not in the graph. - return i; -} - -Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { +Values TranslationRecovery::initializeRandomly( + const std::vector> &relativeTranslations, + std::mt19937 *rng) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly // initializes if not. @@ -135,54 +151,53 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { }; // Loop over measurements and add a random translation - for (auto edge : relativeTranslations_) { + for (auto edge : relativeTranslations) { insert(edge.key1()); insert(edge.key2()); } + return initial; +} + +Values TranslationRecovery::initializeRandomly( + const std::vector> &relativeTranslations) const { + return initializeRandomly(relativeTranslations, &kRandomNumberGenerator); +} + +Values TranslationRecovery::run( + const TranslationEdges &relativeTranslations, const double scale, + const std::vector> &betweenTranslations) const { + // Find edges that have a zero-translation, and recompute relativeTranslations + // and betweenTranslations by retaining only one node for every zero-edge. + DSFMap sameTranslationDSFMap = + getSameTranslationDSFMap(relativeTranslations); + const TranslationEdges nonzeroRelativeTranslations = + removeSameTranslationNodes(relativeTranslations, sameTranslationDSFMap); + const std::vector> nonzeroBetweenTranslations = + removeSameTranslationNodes(betweenTranslations, sameTranslationDSFMap); + + // Create graph of translation factors. + NonlinearFactorGraph graph = buildGraph(nonzeroRelativeTranslations); + + // Add global frame prior and scale (either from betweenTranslations or + // scale). + addPrior(nonzeroRelativeTranslations, scale, nonzeroBetweenTranslations, + &graph); + + // Uses initial values from params if provided. + Values initial = initializeRandomly(nonzeroRelativeTranslations); // 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 (initial.empty() && !sameTranslationDSFMap.sets().empty()) { + for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) { Key optimizedKey = optimizedAndDuplicateKeys.first; initial.insert(optimizedKey, Point3(0, 0, 0)); } } - return initial; -} -Values TranslationRecovery::initializeRandomly() const { - return initializeRandomly(&kRandomNumberGenerator); -} - -Values TranslationRecovery::run( - const std::vector> &betweenTranslations, - const double scale) const { - NonlinearFactorGraph graph = buildGraph(); - addPrior(betweenTranslations, scale, &graph); - const Values initial = initializeRandomly(); LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); Values result = lm.optimize(); - return addSameTranslationNodes(result); -} - -Values TranslationRecovery::addSameTranslationNodes( - const Values &result) const { - Values final_result = result; - // 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 (final_result.exists(duplicateKey)) continue; - final_result.insert(duplicateKey, - final_result.at(optimizedKey)); - } - } - return final_result; + return addSameTranslationNodes(result, sameTranslationDSFMap); } TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 931d072c81..a9f823d1ae 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -69,29 +69,25 @@ class TranslationRecovery { // Parameters. TranslationRecoveryParams params_; - // Map from a key in the graph to a set of keys that share the same - // translation. - std::map> sameTranslationNodes_; - public: /** * @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. - * @param params (optional) parameters for the recovery problem. + * @param params parameters for the recovery problem. */ - TranslationRecovery( - const TranslationEdges &relativeTranslations, - const TranslationRecoveryParams ¶ms = TranslationRecoveryParams()); + TranslationRecovery(const TranslationRecoveryParams ¶ms) + : params_(params) {} + + // Same as above, with default parameters. + TranslationRecovery() = default; /** * @brief Build the factor graph to do the optimization. * * @return NonlinearFactorGraph */ - NonlinearFactorGraph buildGraph() const; + NonlinearFactorGraph buildGraph( + const std::vector> &relativeTranslations) const; /** * @brief Add priors on ednpoints of first measurement edge. @@ -101,8 +97,10 @@ class TranslationRecovery { * @param priorNoiseModel the noise model to use with the prior. */ void addPrior( + const std::vector> &relativeTranslations, + const double scale, const std::vector> &betweenTranslations, - const double scale, NonlinearFactorGraph *graph, + NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01)) const; @@ -112,25 +110,34 @@ class TranslationRecovery { * @param rng random number generator * @return Values */ - Values initializeRandomly(std::mt19937 *rng) const; + Values initializeRandomly( + const std::vector> &relativeTranslations, + std::mt19937 *rng) const; /** * @brief Version of initializeRandomly with a fixed seed. * * @return Values */ - Values initializeRandomly() const; + Values initializeRandomly( + const std::vector> &relativeTranslations) const; /** * @brief Build and optimize factor graph. * + * @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. * @param scale scale for first relative translation which fixes gauge. - * The scale is only used if relativeTranslations in the params is empty. + * The scale is only used if betweenTranslations is empty. + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @return Values */ - Values run( - const std::vector> &betweenTranslations = {}, - const double scale = 1.0) const; + Values run(const TranslationEdges &relativeTranslations, + const double scale = 1.0, + const std::vector> &betweenTranslations = + {}) const; /** * @brief Simulate translation direction measurements @@ -143,25 +150,5 @@ class TranslationRecovery { */ static TranslationEdges SimulateMeasurements( const Values &poses, const std::vector &edges); - - private: - /** - * @brief Gets the key of the variable being optimized among multiple input - * variables that have the same translation. - * - * @param i key of input variable. - * @return Key of optimized variable - same as input if it does not have any - * zero-translation edges. - */ - Key getSameTranslationRootNode(const Key i) const; - - /** - * @brief Adds nodes that were not optimized for because they were connected - * to another node with a zero-translation edge in the input. - * - * @param result optimization problem result - * @return translation estimates for all variables in the input. - */ - Values addSameTranslationNodes(const Values &result) const; }; } // namespace gtsam diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 6c7233a37e..5dd319d301 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -62,13 +62,13 @@ TEST(TranslationRecovery, BAL) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); + TranslationRecovery algorithm; + const auto graph = algorithm.buildGraph(relativeTranslations); EXPECT_LONGS_EQUAL(3, graph.size()); // Run translation recovery const double scale = 2.0; - const auto result = algorithm.run(/*betweenTranslations=*/{}, scale); + const auto result = algorithm.run(relativeTranslations, scale); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); @@ -107,12 +107,12 @@ TEST(TranslationRecovery, TwoPoseTest) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); + TranslationRecovery algorithm; + const auto graph = algorithm.buildGraph(relativeTranslations); EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -145,11 +145,11 @@ TEST(TranslationRecovery, ThreePoseTest) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); + TranslationRecovery algorithm; + const auto graph = algorithm.buildGraph(relativeTranslations); EXPECT_LONGS_EQUAL(3, graph.size()); - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -180,13 +180,9 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); - // There is only 1 non-zero translation edge. - EXPECT_LONGS_EQUAL(1, graph.size()); - + TranslationRecovery algorithm; // Run translation recovery - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -222,12 +218,10 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); - EXPECT_LONGS_EQUAL(3, graph.size()); + TranslationRecovery algorithm; // Run translation recovery - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -251,13 +245,10 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { 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()); + TranslationRecovery algorithm; // Run translation recovery - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -289,8 +280,9 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) { betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), noiseModel::Isotropic::Sigma(3, 1e-2)); - TranslationRecovery algorithm(relativeTranslations); - auto result = algorithm.run(betweenTranslations); + TranslationRecovery algorithm; + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); @@ -322,8 +314,9 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), noiseModel::Constrained::All(3, 1e2)); - TranslationRecovery algorithm(relativeTranslations); - auto result = algorithm.run(betweenTranslations); + TranslationRecovery algorithm; + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); From c0e5ce9ef9da661440d13e47953400d96be9ab54 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 18:28:54 -0700 Subject: [PATCH 09/13] wrapper updates --- gtsam/sfm/sfm.i | 30 ++++++++++++++----- .../examples/TranslationAveragingExample.py | 2 +- .../gtsam/tests/test_TranslationRecovery.py | 10 +++++-- 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 66280037c2..122a9b5a6a 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -265,19 +265,33 @@ class MFAS { class TranslationRecoveryParams { gtsam::LevenbergMarquardtParams lmParams; gtsam::Values initial; + TranslationRecoveryParams(); }; class TranslationRecovery { - TranslationRecovery( + TranslationRecovery(const gtsam::TranslationRecoveryParams& lmParams); + TranslationRecovery(); // default params. + void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + gtsam::NonlinearFactorGraph @graph, + const gtsam::SharedNoiseModel& priorNoiseModel) const; + void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + gtsam::NonlinearFactorGraph @graph) const; + gtsam::NonlinearFactorGraph buildGraph( + const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; + gtsam::Values run( const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::TranslationRecoveryParams& lmParams); - TranslationRecovery(const gtsam::BinaryMeasurementsUnit3& - relativeTranslations); // default params - gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations, - const double scale) const; + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const; // default empty betweenTranslations - // gtsam::Values run(const double scale) const; - gtsam::Values run() const; // default scale = 1.0 + gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale) const; + // default scale = 1.0, empty betweenTranslations + gtsam::Values run( + const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; }; } // namespace gtsam diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 054b61126c..92a7d04e35 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -123,7 +123,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, w_iZj_inliers = filter_outliers(w_iZj_list) # Run the optimizer to obtain translations for normalized directions. - wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run() + wtc_values = gtsam.TranslationRecovery().run(w_iZj_inliers) wTc_values = gtsam.Values() for key in wRc_values.keys(): diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 0fb0518b60..2875e897e5 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -34,8 +34,10 @@ class TestTranslationRecovery(unittest.TestCase): def test_constructor(self): """Construct from binary measurements.""" - algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3()) + algorithm = gtsam.TranslationRecovery() self.assertIsInstance(algorithm, gtsam.TranslationRecovery) + algorithm_params = gtsam.TranslationRecovery(gtsam.TranslationRecoveryParams()) + self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery) def test_run(self): gt_poses = ExampleValues() @@ -44,10 +46,12 @@ def test_run(self): # Set verbosity to Silent for tests lmParams = gtsam.LevenbergMarquardtParams() lmParams.setVerbosityLM("silent") + params = gtsam.TranslationRecoveryParams() + params.lmParams = lmParams - algorithm = gtsam.TranslationRecovery(measurements, lmParams) + algorithm = gtsam.TranslationRecovery(params) scale = 2.0 - result = algorithm.run(scale) + result = algorithm.run(measurements, scale) w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation() w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation() From 3b77df652c8a8840ffa5785d061344553534b441 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 18:36:00 -0700 Subject: [PATCH 10/13] docstring updates --- gtsam/sfm/TranslationRecovery.h | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index a9f823d1ae..3462cce02a 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -11,7 +11,7 @@ /** * @file TranslationRecovery.h - * @author Frank Dellaert + * @author Frank Dellaert, Akshay Krishnan * @date March 2020 * @brief Recovering translations in an epipolar graph when rotations are given. */ @@ -78,12 +78,16 @@ class TranslationRecovery { TranslationRecovery(const TranslationRecoveryParams ¶ms) : params_(params) {} - // Same as above, with default parameters. + /** + * @brief Default constructor. + */ TranslationRecovery() = default; /** * @brief Build the factor graph to do the optimization. * + * @param relativeTranslations unit translation directions between + * translations to be estimated * @return NonlinearFactorGraph */ NonlinearFactorGraph buildGraph( @@ -92,8 +96,12 @@ class TranslationRecovery { /** * @brief Add priors on ednpoints of first measurement edge. * + * @param relativeTranslations unit translation directions between + * translations to be estimated * @param scale scale for first relative translation which fixes gauge. * @param graph factor graph to which prior is added. + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param priorNoiseModel the noise model to use with the prior. */ void addPrior( @@ -105,8 +113,11 @@ class TranslationRecovery { noiseModel::Isotropic::Sigma(3, 0.01)) const; /** - * @brief Create random initial translations. + * @brief Create random initial translations. Uses inial values from params if + * provided. * + * @param relativeTranslations unit translation directions between + * translations to be estimated * @param rng random number generator * @return Values */ @@ -115,8 +126,11 @@ class TranslationRecovery { std::mt19937 *rng) const; /** - * @brief Version of initializeRandomly with a fixed seed. + * @brief Version of initializeRandomly with a fixed seed. Uses initial values + * from params if provided. * + * @param relativeTranslations unit translation directions between + * translations to be estimated * @return Values */ Values initializeRandomly( From e17eddf99f8eac95077887b871f1859f7d5eb802 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 9 May 2022 22:46:16 -0700 Subject: [PATCH 11/13] move values from params to run, remove params struct --- gtsam/sfm/TranslationRecovery.cpp | 20 ++++---- gtsam/sfm/TranslationRecovery.h | 46 +++++++++---------- gtsam/sfm/sfm.i | 12 ++--- .../gtsam/tests/test_TranslationRecovery.py | 4 +- 4 files changed, 41 insertions(+), 41 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 122b17ce68..810fe7de98 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -134,15 +134,15 @@ void TranslationRecovery::addPrior( Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, - std::mt19937 *rng) const { + std::mt19937 *rng, const Values &initialValues) 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 = [&](Key j) { if (initial.exists(j)) return; - if (params_.initial.exists(j)) { - initial.insert(j, params_.initial.at(j)); + if (initialValues.exists(j)) { + initial.insert(j, initialValues.at(j)); } else { initial.insert( j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); @@ -159,13 +159,16 @@ Values TranslationRecovery::initializeRandomly( } Values TranslationRecovery::initializeRandomly( - const std::vector> &relativeTranslations) const { - return initializeRandomly(relativeTranslations, &kRandomNumberGenerator); + const std::vector> &relativeTranslations, + const Values &initialValues) const { + return initializeRandomly(relativeTranslations, &kRandomNumberGenerator, + initialValues); } Values TranslationRecovery::run( const TranslationEdges &relativeTranslations, const double scale, - const std::vector> &betweenTranslations) const { + const std::vector> &betweenTranslations, + const Values &initialValues) const { // Find edges that have a zero-translation, and recompute relativeTranslations // and betweenTranslations by retaining only one node for every zero-edge. DSFMap sameTranslationDSFMap = @@ -184,7 +187,8 @@ Values TranslationRecovery::run( &graph); // Uses initial values from params if provided. - Values initial = initializeRandomly(nonzeroRelativeTranslations); + Values initial = + initializeRandomly(nonzeroRelativeTranslations, initialValues); // If there are no valid edges, but zero-distance edges exist, initialize one // of the nodes in a connected component of zero-distance edges. @@ -195,7 +199,7 @@ Values TranslationRecovery::run( } } - LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); + LevenbergMarquardtOptimizer lm(graph, initial, lmParams_); Values result = lm.optimize(); return addSameTranslationNodes(result, sameTranslationDSFMap); } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 3462cce02a..7863f51339 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -28,15 +28,6 @@ namespace gtsam { -// Parameters for the Translation Recovery problem. -struct TranslationRecoveryParams { - // LevenbergMarquardtParams for optimization. - LevenbergMarquardtParams lmParams; - - // Initial values, random intialization will be used if not provided. - Values initial; -}; - // Set up an optimization problem for the unknown translations Ti in the world // coordinate frame, given the known camera attitudes wRi with respect to the // world frame, and a set of (noisy) translation directions of type Unit3, @@ -67,16 +58,16 @@ class TranslationRecovery { TranslationEdges relativeTranslations_; // Parameters. - TranslationRecoveryParams params_; + LevenbergMarquardtParams lmParams_; public: /** * @brief Construct a new Translation Recovery object * - * @param params parameters for the recovery problem. + * @param lmParams parameters for optimization. */ - TranslationRecovery(const TranslationRecoveryParams ¶ms) - : params_(params) {} + TranslationRecovery(const LevenbergMarquardtParams &lmParams) + : lmParams_(lmParams) {} /** * @brief Default constructor. @@ -94,7 +85,11 @@ class TranslationRecovery { const std::vector> &relativeTranslations) const; /** - * @brief Add priors on ednpoints of first measurement edge. + * @brief Add 3 factors to the graph: + * - A prior on the first point to lie at (0, 0, 0) + * - If betweenTranslations is non-empty, between factors provided by it. + * - If betweenTranslations is empty, a prior on scale of the first + * relativeTranslations edge. * * @param relativeTranslations unit translation directions between * translations to be estimated @@ -113,28 +108,29 @@ class TranslationRecovery { noiseModel::Isotropic::Sigma(3, 0.01)) const; /** - * @brief Create random initial translations. Uses inial values from params if - * provided. + * @brief Create random initial translations. * * @param relativeTranslations unit translation directions between * translations to be estimated * @param rng random number generator + * @param intialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, - std::mt19937 *rng) const; + std::mt19937 *rng, const Values &initialValues = Values()) const; /** - * @brief Version of initializeRandomly with a fixed seed. Uses initial values - * from params if provided. + * @brief Version of initializeRandomly with a fixed seed. * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( - const std::vector> &relativeTranslations) const; + const std::vector> &relativeTranslations, + const Values &initialValues = Values()) const; /** * @brief Build and optimize factor graph. @@ -146,12 +142,14 @@ class TranslationRecovery { * The scale is only used if betweenTranslations is empty. * @param betweenTranslations relative translations (with scale) between 2 * points in world coordinate frame known a priori. + * @param initialValues intial values for optimization. Initializes randomly + * if not provided. * @return Values */ - Values run(const TranslationEdges &relativeTranslations, - const double scale = 1.0, - const std::vector> &betweenTranslations = - {}) const; + Values run( + const TranslationEdges &relativeTranslations, const double scale = 1.0, + const std::vector> &betweenTranslations = {}, + const Values &initialValues = Values()) const; /** * @brief Simulate translation direction measurements diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 122a9b5a6a..33f23614a5 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -262,14 +262,9 @@ class MFAS { }; #include -class TranslationRecoveryParams { - gtsam::LevenbergMarquardtParams lmParams; - gtsam::Values initial; - TranslationRecoveryParams(); -}; class TranslationRecovery { - TranslationRecovery(const gtsam::TranslationRecoveryParams& lmParams); + TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams); TranslationRecovery(); // default params. void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, @@ -282,6 +277,11 @@ class TranslationRecovery { gtsam::NonlinearFactorGraph @graph) const; gtsam::NonlinearFactorGraph buildGraph( const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; + gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + const gtsam::Values& initialValues) const; + // default random initial values gtsam::Values run( const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 2875e897e5..0908f0a693 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -46,10 +46,8 @@ def test_run(self): # Set verbosity to Silent for tests lmParams = gtsam.LevenbergMarquardtParams() lmParams.setVerbosityLM("silent") - params = gtsam.TranslationRecoveryParams() - params.lmParams = lmParams - algorithm = gtsam.TranslationRecovery(params) + algorithm = gtsam.TranslationRecovery(lmParams) scale = 2.0 result = algorithm.run(measurements, scale) From 50b77ceb551b60a35b2fd8bc272b753b9edb506d Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 10 May 2022 08:36:16 -0700 Subject: [PATCH 12/13] fixing merge conflict in wrapper --- gtsam/sfm/sfm.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index df0cb279b9..83bd07b13f 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -104,6 +104,7 @@ class BinaryMeasurementsPoint3 { size_t size() const; gtsam::BinaryMeasurement at(size_t idx) const; void push_back(const gtsam::BinaryMeasurement& measurement); +}; class BinaryMeasurementsRot3 { BinaryMeasurementsRot3(); From 29787373a6ec40cbb526eed1f3616fb892bdd621 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 10 May 2022 17:46:52 -0700 Subject: [PATCH 13/13] update python test constructor --- python/gtsam/tests/test_TranslationRecovery.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 0908f0a693..99fbce89e2 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -36,7 +36,7 @@ def test_constructor(self): """Construct from binary measurements.""" algorithm = gtsam.TranslationRecovery() self.assertIsInstance(algorithm, gtsam.TranslationRecovery) - algorithm_params = gtsam.TranslationRecovery(gtsam.TranslationRecoveryParams()) + algorithm_params = gtsam.TranslationRecovery(gtsam.LevenbergMarquardtParams()) self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery) def test_run(self):