From 76297ea2cc90cf1a351b33a2184555e429bc3827 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 31 May 2014 17:15:29 -0400
Subject: [PATCH] Adding noise does not help much

---
 gtsam/nonlinear/tests/timeLago.cpp | 15 ++++++++++++---
 1 file changed, 12 insertions(+), 3 deletions(-)

diff --git a/gtsam/nonlinear/tests/timeLago.cpp b/gtsam/nonlinear/tests/timeLago.cpp
index 85425f64b..d3c86d0a4 100644
--- a/gtsam/nonlinear/tests/timeLago.cpp
+++ b/gtsam/nonlinear/tests/timeLago.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/lago.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/linear/Sampler.h>
 #include <gtsam/base/timing.h>
 
 #include <iostream>
@@ -32,11 +33,19 @@ int main(int argc, char *argv[]) {
   size_t trials = 1;
 
   // read graph
-  Values::shared_ptr initial;
+  Values::shared_ptr solution;
   NonlinearFactorGraph::shared_ptr g;
   string inputFile = findExampleDataFile("w10000");
   SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
-  boost::tie(g, initial) = load2D(inputFile, model);
+  boost::tie(g, solution) = load2D(inputFile, model);
+
+  // add noise to create initial estimate
+  Values initial;
+  Sampler sampler(42u);
+  Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
+  SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0));
+  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, poses)
+    initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise)));
 
   // Add prior on the pose having index (key) = 0
   noiseModel::Diagonal::shared_ptr priorModel = //
@@ -60,7 +69,7 @@ int main(int argc, char *argv[]) {
 
     {
       gttic_(optimize);
-      GaussNewtonOptimizer optimizer(*g, *initial);
+      GaussNewtonOptimizer optimizer(*g, initial);
       Values result = optimizer.optimize();
     }