From d8f3ca46a4966f0c13250caa929de74fb441966f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 8 Mar 2020 01:58:22 -0500
Subject: [PATCH] Added initalizeRandomly

---
 tests/testTranslationRecovery.cpp | 29 ++++++++++++++++++++++++++---
 1 file changed, 26 insertions(+), 3 deletions(-)

diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp
index 330a507e5..f513184e3 100644
--- a/tests/testTranslationRecovery.cpp
+++ b/tests/testTranslationRecovery.cpp
@@ -167,6 +167,29 @@ class TranslationRecovery {
     return graph;
   }
 
+  /**
+   * @brief Create random initial translations.
+   *
+   * @return Values
+   */
+  Values initalizeRandomly() const {
+    Values initial;
+    auto insert = [&initial](Key j) {
+      if (!initial.exists(j)) {
+        initial.insert<Point3>(j, Vector3::Random());
+      }
+    };
+
+    // Loop over measurements and add a random translation
+    for (auto edge : relativeTranslations_) {
+      Key a, b;
+      std::tie(a, b) = edge.first;
+      insert(a);
+      insert(b);
+    }
+    return initial;
+  }
+
   /**
    * @brief Build and optimize factor graph.
    *
@@ -175,11 +198,11 @@ class TranslationRecovery {
    */
   Values run(const double scale = 1.0) const {
     const auto graph = buildGraph();
-    //   Values initial = randomTranslations();
+    const Values initial = initalizeRandomly();
 
-    //   LevenbergMarquardtOptimizer lm(graph, initial);
+    LevenbergMarquardtOptimizer lm(graph, initial);
 
-    Values result;  // = lm.optimize();
+    Values result = lm.optimize();
 
     return result;
   }