diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 319129840..d4100b00a 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -11,13 +11,12 @@ /** * @file TranslationRecovery.cpp - * @author Frank Dellaert + * @author Frank Dellaert, Akshay Krishnan * @date March 2020 * @brief Source code for recovering translations when rotations are given */ -#include - +#include #include #include #include @@ -27,11 +26,45 @@ #include #include #include +#include #include +#include +#include + using namespace gtsam; using namespace std; +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. + DSFMap sameTranslationDSF; + for (const auto &edge : relativeTranslations) { + Key key1 = sameTranslationDSF.find(edge.key1()); + Key key2 = sameTranslationDSF.find(edge.key2()); + if (key1 != key2 && edge.measured().equals(Unit3(0.0, 0.0, 0.0))) { + sameTranslationDSF.merge(key1, key2); + } + } + // 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(); +} + NonlinearFactorGraph TranslationRecovery::buildGraph() const { NonlinearFactorGraph graph; @@ -44,13 +77,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { return graph; } -void TranslationRecovery::addPrior(const double scale, - NonlinearFactorGraph *graph, - const SharedNoiseModel &priorNoiseModel) const { +void TranslationRecovery::addPrior( + const double scale, NonlinearFactorGraph *graph, + const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); - graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); - graph->emplace_shared >(edge->key2(), scale * edge->measured().point3(), - edge->noiseModel()); + graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), + priorNoiseModel); + graph->emplace_shared >( + edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); } Values TranslationRecovery::initalizeRandomly() const { @@ -77,6 +111,19 @@ Values TranslationRecovery::run(const double scale) const { const Values initial = initalizeRandomly(); LevenbergMarquardtOptimizer lm(graph, initial, params_); 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; } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index d5538f91b..c99836853 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -16,14 +16,16 @@ * @brief Recovering translations in an epipolar graph when rotations are given. */ +#include +#include +#include +#include + #include #include #include #include -#include -#include - namespace gtsam { // Set up an optimization problem for the unknown translations Ti in the world @@ -52,23 +54,30 @@ class TranslationRecovery { using TranslationEdges = std::vector>; private: + // 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_; + 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. + * 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. + * default LM parameters. */ - TranslationRecovery(const TranslationEdges &relativeTranslations, - const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()) - : relativeTranslations_(relativeTranslations), params_(lmParams) {} + TranslationRecovery( + const TranslationEdges &relativeTranslations, + const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()); /** * @brief Build the factor graph to do the optimization. @@ -108,8 +117,8 @@ class TranslationRecovery { * * @param poses SE(3) ground truth poses stored as Values * @param edges pairs (a,b) for which a measurement w_aZb will be generated. - * @return TranslationEdges vector of binary measurements where the keys are - * the cameras and the measurement is the simulated Unit3 translation + * @return TranslationEdges vector of binary measurements where the keys are + * the cameras and the measurement is the simulated Unit3 translation * direction between the cameras. */ static TranslationEdges SimulateMeasurements( diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index eb34ba803..7260fd5af 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -11,19 +11,29 @@ /** * @file testTranslationRecovery.cpp - * @author Frank Dellaert + * @author Frank Dellaert, Akshay Krishnan * @date March 2020 * @brief test recovering translations when rotations are given. */ -#include - #include + +#include #include using namespace std; using namespace gtsam; +// Returns the Unit3 direction as measured in the binary measurement, but +// computed from the input poses. Helper function used in the unit tests. +Unit3 GetDirectionFromPoses(const Values& poses, + const BinaryMeasurement& unitTranslation) { + const Pose3 wTa = poses.at(unitTranslation.key1()), + wTb = poses.at(unitTranslation.key2()); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + return Unit3(Tb - Ta); +} + /* ************************************************************************* */ // We read the BAL file, which has 3 cameras in it, with poses. We then assume // the rotations are correct, but translations have to be estimated from @@ -48,43 +58,186 @@ TEST(TranslationRecovery, BAL) { const auto relativeTranslations = TranslationRecovery::SimulateMeasurements( poses, {{0, 1}, {0, 2}, {1, 2}}); - // Check - Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test - for(auto& unitTranslation : relativeTranslations) { - const Pose3 wTa = poses.at(unitTranslation.key1()), - wTb = poses.at(unitTranslation.key2()); - const Point3 Ta = wTa.translation(), Tb = wTb.translation(); - const Unit3 w_aZb = unitTranslation.measured(); - EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); - if(unitTranslation.key1() == 0 && unitTranslation.key2() == 1) { - w_aZb_stored = unitTranslation.measured(); - } + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); } TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); EXPECT_LONGS_EQUAL(3, graph.size()); - // Translation recovery, version 1 + // Run translation recovery const double scale = 2.0; const auto result = algorithm.run(scale); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at(1))); + EXPECT(assert_equal( + Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])), + result.at(1))); // Check that the third translations is correct Point3 Ta = poses.at(0).translation(); Point3 Tb = poses.at(1).translation(); Point3 Tc = poses.at(2).translation(); - Point3 expected = - (Tc - Ta) * (scale / (Tb - Ta).norm()); + Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); EXPECT(assert_equal(expected, result.at(2), 1e-4)); // TODO(frank): how to get stats back? // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); } +TEST(TranslationRecovery, TwoPoseTest) { + // Create a dataset with 2 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // + // 0 and 1 face in the same direction but have a translation offset. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(1, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(/*scale=*/3.0); + + // Check result for first two translations, determined by prior + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); +} + +TEST(TranslationRecovery, ThreePoseTest) { + // 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}, {1, 3}, {3, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(3, graph.size()); + + const auto result = algorithm.run(/*scale=*/3.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at(3))); +} + +TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // 2 <| + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with little FOV overlap. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + // There is only 1 non-zero translation edge. + EXPECT_LONGS_EQUAL(1, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(/*scale=*/3.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(2))); +} + +TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { + // Create a dataset with 4 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ 2 <| + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with very little FOV overlap. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(3, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(/*scale=*/4.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(2))); + EXPECT(assert_equal(Point3(2, -2, 0), result.at(3))); +} + /* ************************************************************************* */ int main() { TestResult tr;