diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 810fe7de98..c7ef2e526d 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -119,21 +119,24 @@ void TranslationRecovery::addPrior( 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()); + return; + } + + // Add between factors for optional relative translations. + for (auto prior_edge : betweenTranslations) { + graph->emplace_shared>( + prior_edge.key1(), prior_edge.key2(), prior_edge.measured(), + prior_edge.noiseModel()); } } Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly @@ -155,14 +158,20 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + // There may be nodes in betweenTranslations that do not have a measurement. + for (auto edge : betweenTranslations) { + insert(edge.key1()); + insert(edge.key2()); + } return initial; } Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues) const { - return initializeRandomly(relativeTranslations, &kRandomNumberGenerator, - initialValues); + return initializeRandomly(relativeTranslations, betweenTranslations, + &kRandomNumberGenerator, initialValues); } Values TranslationRecovery::run( @@ -187,8 +196,8 @@ Values TranslationRecovery::run( &graph); // Uses initial values from params if provided. - Values initial = - initializeRandomly(nonzeroRelativeTranslations, initialValues); + Values initial = initializeRandomly( + nonzeroRelativeTranslations, nonzeroBetweenTranslations, 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. diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 7863f51339..44a5ef43ef 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -112,12 +112,15 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param rng random number generator * @param intialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues = Values()) const; /** @@ -125,11 +128,14 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues = Values()) const; /** @@ -137,11 +143,15 @@ 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. + * measurement is a point in 3D. If a relative translation magnitude is zero, + * it is treated as a hard same-point constraint (the result of all nodes + * connected by a zero-magnitude edge will be the same). * @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. + * points in world coordinate frame known a priori. Unlike + * relativeTranslations, zero-magnitude betweenTranslations are not treated as + * hard constraints. * @param initialValues intial values for optimization. Initializes randomly * if not provided. * @return Values diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 5dd319d301..15f1caa1b4 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -323,6 +323,36 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { 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, NodeWithBetweenFactorAndNoMeasurements) { + // Checks that valid results are obtained when a between translation edge is + // provided with a node that does not have any other relative translations. + 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))); + poses.insert(4, Pose3(Rot3(), Point3(1, 2, 1))); + + 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)); + // Node 4 only has this between translation prior, no relative translations. + betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1)); + + 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)); + EXPECT(assert_equal(Point3(1, 2, 1), result.at(4), 1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr;