diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 2e81c2d561..810fe7de98 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -21,13 +21,16 @@ #include #include #include +#include #include #include #include #include #include #include +#include #include +#include #include #include @@ -38,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 LevenbergMarquardtParams &lmParams) - : params_(lmParams) { - // 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()); @@ -56,94 +56,152 @@ 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 all relative translation edges - for (auto edge : relativeTranslations_) { + // Add translation factors for input translation directions. + for (auto edge : relativeTranslations) { graph.emplace_shared(edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); } - return graph; } void TranslationRecovery::addPrior( - const double scale, NonlinearFactorGraph *graph, + const std::vector> &relativeTranslations, + const double scale, + const std::vector> &betweenTranslations, + NonlinearFactorGraph *graph, 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()); + 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) { + graph->emplace_shared>( + edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); + } + + // Add a scale prior only if no other between factors were added. + if (betweenTranslations.empty()) { + graph->emplace_shared>( + edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); + } } -Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { +Values TranslationRecovery::initializeRandomly( + const std::vector> &relativeTranslations, + 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)) { + if (initial.exists(j)) return; + if (initialValues.exists(j)) { + initial.insert(j, initialValues.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 - for (auto edge : relativeTranslations_) { + for (auto edge : relativeTranslations) { insert(edge.key1()); insert(edge.key2()); } + return initial; +} + +Values TranslationRecovery::initializeRandomly( + 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 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 = + 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, 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. - 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 double scale) const { - auto graph = buildGraph(); - addPrior(scale, &graph); - const Values initial = initializeRandomly(); - LevenbergMarquardtOptimizer lm(graph, initial, params_); + LevenbergMarquardtOptimizer lm(graph, initial, lmParams_); Values result = lm.optimize(); - - // Nodes that were not optimized are stored in sameTranslationNodes_ as a map - // from a key that was optimized to keys that were not optimized. Iterate over - // map and add results for keys not optimized. - for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { - Key optimizedKey = optimizedAndDuplicateKeys.first; - std::set duplicateKeys = optimizedAndDuplicateKeys.second; - // Add the result for the duplicate key if it does not already exist. - for (const Key duplicateKey : duplicateKeys) { - if (result.exists(duplicateKey)) continue; - result.insert(duplicateKey, result.at(optimizedKey)); - } - } - return result; + return addSameTranslationNodes(result, sameTranslationDSFMap); } TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 30c9a14e39..7863f51339 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. */ @@ -57,68 +57,99 @@ class TranslationRecovery { // Translation directions between camera pairs. TranslationEdges relativeTranslations_; - // Parameters used by the LM Optimizer. - LevenbergMarquardtParams params_; - - // Map from a key in the graph to a set of keys that share the same - // translation. - std::map> sameTranslationNodes_; + // Parameters. + LevenbergMarquardtParams lmParams_; 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 lmParams (optional) gtsam::LavenbergMarquardtParams that can be - * used to modify the parameters for the LM optimizer. By default, uses the - * default LM parameters. + * @param lmParams parameters for optimization. + */ + TranslationRecovery(const LevenbergMarquardtParams &lmParams) + : lmParams_(lmParams) {} + + /** + * @brief Default constructor. */ - TranslationRecovery( - const TranslationEdges &relativeTranslations, - const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()); + 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() const; + NonlinearFactorGraph buildGraph( + 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 * @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(const double scale, NonlinearFactorGraph *graph, - const SharedNoiseModel &priorNoiseModel = - noiseModel::Isotropic::Sigma(3, 0.01)) const; + void addPrior( + const std::vector> &relativeTranslations, + const double scale, + const std::vector> &betweenTranslations, + NonlinearFactorGraph *graph, + const SharedNoiseModel &priorNoiseModel = + noiseModel::Isotropic::Sigma(3, 0.01)) const; /** * @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(std::mt19937 *rng) const; + Values initializeRandomly( + const std::vector> &relativeTranslations, + std::mt19937 *rng, const Values &initialValues = Values()) const; /** * @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; + Values initializeRandomly( + const std::vector> &relativeTranslations, + const Values &initialValues = Values()) 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 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 double scale = 1.0) 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 19e21b10c0..83bd07b13f 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,6 +4,8 @@ namespace gtsam { +#include +#include #include class SfmTrack { SfmTrack(); @@ -88,6 +90,7 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +typedef gtsam::BinaryMeasurement BinaryMeasurementPoint3; class BinaryMeasurementsUnit3 { BinaryMeasurementsUnit3(); @@ -96,6 +99,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); +}; + class BinaryMeasurementsRot3 { BinaryMeasurementsRot3(); size_t size() const; @@ -154,8 +164,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; @@ -268,15 +278,36 @@ class MFAS { }; #include + class TranslationRecovery { - TranslationRecovery( + TranslationRecovery(const gtsam::LevenbergMarquardtParams& 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 double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + const gtsam::Values& initialValues) const; + // default random initial values + gtsam::Values run( const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::LevenbergMarquardtParams& lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3& - relativeTranslations); // default LevenbergMarquardtParams - gtsam::Values run(const double scale) const; - gtsam::Values run() const; // default scale = 1.0 + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const; + // default empty betweenTranslations + 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/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7c4d7fea71..95e750674d 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>; using BinaryMeasurementsRot3 = std::vector>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 0c8dc314be..05606c5844 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::BinaryMeasurementsRot3 gtsam::DiscreteKey @@ -137,6 +138,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::Pose3Vector gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue + gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 gtsam::CameraSetCal3_S2 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/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 90a2b8417f..311b2c59b4 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_vector > >( diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 0fb0518b60..99fbce89e2 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.LevenbergMarquardtParams()) + self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery) def test_run(self): gt_poses = ExampleValues() @@ -45,9 +47,9 @@ def test_run(self): lmParams = gtsam.LevenbergMarquardtParams() lmParams.setVerbosityLM("silent") - algorithm = gtsam.TranslationRecovery(measurements, lmParams) + algorithm = gtsam.TranslationRecovery(lmParams) 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() diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 833f11355d..5dd319d301 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -17,8 +17,8 @@ */ #include -#include #include +#include #include using namespace std; @@ -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(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(/*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(/*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(/*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(/*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(/*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)); @@ -265,6 +256,73 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { EXPECT(assert_equal(Point3(0, 0, 0), result.at(2), 1e-8)); } +TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) { + // 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}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), + noiseModel::Isotropic::Sigma(3, 1e-2)); + + 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)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + 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}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); + + 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)); + 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;