diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index aa75b8fc6..122b17ce6 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()); - if (key1 == key2) continue; - relativeTranslations_.emplace_back(key1, key2, edge.measured(), - edge.noiseModel()); - } - // Store the DSF map for post-processing results. - sameTranslationNodes_ = sameTranslationDSF.sets(); + return sameTranslationDSF; } -NonlinearFactorGraph TranslationRecovery::buildGraph() const { +// 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; + 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)); + } + } + return final_result; +} + +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()); } - - // If there are no valid edges, but zero-distance edges exist, initialize one - // of the nodes in a connected component of zero-distance edges. - if (initial.empty() && !sameTranslationNodes_.empty()) { - for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { - Key optimizedKey = optimizedAndDuplicateKeys.first; - initial.insert(optimizedKey, Point3(0, 0, 0)); - } - } return initial; } -Values TranslationRecovery::initializeRandomly() const { - return initializeRandomly(&kRandomNumberGenerator); +Values TranslationRecovery::initializeRandomly( + const std::vector> &relativeTranslations) const { + return initializeRandomly(relativeTranslations, &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); -} + 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); -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)); + // 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() && !sameTranslationDSFMap.sets().empty()) { + for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) { + Key optimizedKey = optimizedAndDuplicateKeys.first; + initial.insert(optimizedKey, Point3(0, 0, 0)); } } - return final_result; + + LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); + Values result = lm.optimize(); + return addSameTranslationNodes(result, sameTranslationDSFMap); } TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 931d072c8..a9f823d1a 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 6c7233a37..5dd319d30 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));