From 3d06a737bffa216954c89300accb56eaab9ff355 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Wed, 10 Sep 2014 09:35:49 -0400
Subject: [PATCH] added possibility to specify number of iterations

---
 examples/Pose2SLAMExample_g2o.cpp        | 12 +++-
 examples/Pose3SLAMExample_changeKeys.cpp | 73 ++++++++++++++++++++++++
 gtsam/slam/InitializePose3.cpp           |  2 +-
 3 files changed, 85 insertions(+), 2 deletions(-)
 create mode 100644 examples/Pose3SLAMExample_changeKeys.cpp

diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp
index b46a53198..8f1a53a66 100644
--- a/examples/Pose2SLAMExample_g2o.cpp
+++ b/examples/Pose2SLAMExample_g2o.cpp
@@ -45,11 +45,21 @@ int main(const int argc, const char *argv[]) {
       noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
 
+  GaussNewtonParams params;
+  params.setVerbosity("TERMINATION");
+  if (argc == 4) {
+    params.maxIterations = atoi(argv[3]);
+    std::cout << "User required to perform " << params.maxIterations << " iterations "<< std::endl;
+  }
+
   std::cout << "Optimizing the factor graph" << std::endl;
-  GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
+  GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
+  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "final error=" <<graph->error(result)<< std::endl;
+
   if (argc < 3) {
     result.print("result");
   } else {
diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp
new file mode 100644
index 000000000..1e28c2097
--- /dev/null
+++ b/examples/Pose3SLAMExample_changeKeys.cpp
@@ -0,0 +1,73 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Pose3SLAMExample_initializePose3.cpp
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o rewritted.g2o
+ * @date Aug 25, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/slam/InitializePose3.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <fstream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(const int argc, const char *argv[]) {
+
+  // Read graph from file
+  string g2oFile;
+  if (argc < 2)
+    g2oFile = findExampleDataFile("pose3example.txt");
+  else
+    g2oFile = argv[1];
+
+  NonlinearFactorGraph::shared_ptr graph;
+  Values::shared_ptr initial;
+  bool is3D = true;
+  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+
+  Key firstKey = 8646911284551352320;
+
+  std::cout << "Using reference key: " <<  firstKey  << std::endl;
+
+  if (argc < 3) {
+    std::cout << "Please provide output file to write "   << std::endl;
+  } else {
+    const string inputFileRewritten = argv[2];
+    std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
+    // Additional: rewrite input with simplified keys 0,1,...
+    Values simpleInitial;
+    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+      Key key = key_value.key + firstKey;
+      simpleInitial.insert(key, initial->at(key_value.key));
+    }
+    NonlinearFactorGraph simpleGraph;
+    BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
+      boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+          boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+      if (pose3Between){
+        Key key1 = pose3Between->key1() + firstKey;
+        Key key2 = pose3Between->key2() + firstKey;
+        NonlinearFactor::shared_ptr simpleFactor(
+            new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
+        simpleGraph.add(simpleFactor);
+      }
+    }
+    writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
+  }
+  return 0;
+}
diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index f1415a8fa..c571cd8e7 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -334,7 +334,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 
   // Create optimizer
   GaussNewtonParams params;
-  bool singleIter = false;
+  bool singleIter = true;
   if(singleIter){
     params.maxIterations = 1;
   }else{