From e96ceb2b4f6fb86e2d3df29857844feca8a9c566 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Thu, 16 Oct 2014 14:39:23 -0400
Subject: [PATCH] extended example for robust kernels

---
 examples/Pose2SLAMExample_g2o.cpp | 48 ++++++++++++++++++++++++-------
 1 file changed, 37 insertions(+), 11 deletions(-)

diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp
index 8f1a53a66..8d8f2edc1 100644
--- a/examples/Pose2SLAMExample_g2o.cpp
+++ b/examples/Pose2SLAMExample_g2o.cpp
@@ -26,30 +26,53 @@
 using namespace std;
 using namespace gtsam;
 
+// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
 int main(const int argc, const char *argv[]) {
 
-  // Read graph from file
-  string g2oFile;
-  if (argc < 2)
-    g2oFile = findExampleDataFile("noisyToyGraph.txt");
-  else
-    g2oFile = argv[1];
+  string kernelType = "none";
+  int maxIterations = 100; // default
+  string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
 
+  // Parse user's inputs
+  if (argc > 1){
+    g2oFile = argv[1]; // input dataset filename
+    // outputFile = g2oFile = argv[2]; // done later
+  }
+  if (argc > 3){
+    maxIterations = atoi(argv[3]); // user can specify either tukey or huber
+  }
+  if (argc > 4){
+    kernelType = argv[4]; // user can specify either tukey or huber
+  }
+
+  // reading file and creating factor graph
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
-  boost::tie(graph, initial) = readG2o(g2oFile);
+  bool is3D = false;
+  if(kernelType.compare("none") == 0){
+    boost::tie(graph, initial) = readG2o(g2oFile,is3D);
+  }
+  if(kernelType.compare("huber") == 0){
+    std::cout << "Using robust kernel: huber " << std::endl;
+    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
+  }
+  if(kernelType.compare("tukey") == 0){
+    std::cout << "Using robust kernel: tukey " << std::endl;
+    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
+  }
 
   // Add prior on the pose having index (key) = 0
   NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  std::cout << "Adding prior on pose 0 " << std::endl;
 
   GaussNewtonParams params;
   params.setVerbosity("TERMINATION");
-  if (argc == 4) {
-    params.maxIterations = atoi(argv[3]);
-    std::cout << "User required to perform " << params.maxIterations << " iterations "<< std::endl;
+  if (argc > 3) {
+    params.maxIterations = maxIterations;
+    std::cout << "User required to perform maximum  " << params.maxIterations << " iterations "<< std::endl;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
@@ -65,7 +88,10 @@ int main(const int argc, const char *argv[]) {
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(*graph, result, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, result, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;