From a2f99ac71cbaad5b92dad26ff2f72c4ca872a7ca Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Fri, 5 Mar 2021 07:21:38 +0000
Subject: [PATCH] unit test + fix segfault

---
 gtsam/sfm/TranslationRecovery.cpp | 10 ++++++++++
 tests/testTranslationRecovery.cpp | 29 +++++++++++++++++++++++++++++
 2 files changed, 39 insertions(+)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index d4100b00a..7f788ff84 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -81,6 +81,7 @@ void TranslationRecovery::addPrior(
     const double scale, NonlinearFactorGraph *graph,
     const SharedNoiseModel &priorNoiseModel) const {
   auto edge = relativeTranslations_.begin();
+  if(edge == relativeTranslations_.end()) return;
   graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0),
                                               priorNoiseModel);
   graph->emplace_shared<PriorFactor<Point3> >(
@@ -102,6 +103,15 @@ Values TranslationRecovery::initalizeRandomly() const {
     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<Point3>(optimizedKey, Point3(0, 0, 0));
+    }
+  }
   return initial;
 }
 
diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp
index 7260fd5af..9a4e2b545 100644
--- a/tests/testTranslationRecovery.cpp
+++ b/tests/testTranslationRecovery.cpp
@@ -238,6 +238,35 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
   EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3)));
 }
 
+TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
+  Values poses;
+  poses.insert<Pose3>(0, Pose3(Rot3::RzRyRx(-M_PI / 6, 0, 0), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0)));
+
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}, {2, 0}});
+
+  // Check simulated measurements.
+  for (auto& unitTranslation : relativeTranslations) {
+    EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), 
+			    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());
+  
+  // Run translation recovery
+  const auto result = algorithm.run(/*scale=*/4.0);
+  
+  // Check result
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2)));
+}
+                                                   
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;