From 5ec6127c0b5bebe03149068f276e53870a0aa8bb Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 18 Jun 2022 11:35:46 -0700
Subject: [PATCH] add unit test, update doc

---
 gtsam/sfm/TranslationRecovery.cpp |  1 +
 tests/testTranslationRecovery.cpp | 30 ++++++++++++++++++++++++++++++
 2 files changed, 31 insertions(+)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index 8f1108806..28c2b2e8a 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -156,6 +156,7 @@ 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());
diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp
index 5dd319d30..15f1caa1b 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<Point3>(1), 1e-4));
   EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(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<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
+  poses.insert<Pose3>(4, Pose3(Rot3(), Point3(1, 2, 1)));
+
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {0, 3}, {1, 3}});
+
+  std::vector<BinaryMeasurement<Point3>> 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<Point3>(0), 1e-4));
+  EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
+  EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
+  EXPECT(assert_equal(Point3(1, 2, 1), result.at<Point3>(4), 1e-4));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;