Merge branch 'outlierRejectionTest'

Conflicts:
	.cproject

examples/Pose2SLAMExample_g2o.cpp

@@ -0,0 +1,61 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Pose2SLAMExample.cpp
+ * @brief A 2D Pose SLAM example that reads input from g2o and uses robust kernels in optimization
+ * @date May 15, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/Values.h>
+#include <fstream>
+#include <sstream>
+
+using namespace std;
+using namespace gtsam;
+
+
+int main(const int argc, const char *argv[]){
+
+  if (argc < 2)
+    std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl;
+  const string g2oFile = argv[1];
+
+  NonlinearFactorGraph graph;
+  Values initial;
+  readG2o(g2oFile, graph, initial);
+
+  // Add prior on the pose having index (key) = 0
+  NonlinearFactorGraph graphWithPrior = graph;
+  noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001));
+  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+
+  std::cout << "Optimizing the factor graph" << std::endl;
+  GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters);
+  Values result = optimizer.optimize();
+  std::cout << "Optimization complete" << std::endl;
+
+  const string outputFile = argv[2];
+  std::cout << "Writing results to file: " << outputFile << std::endl;
+  writeG2o(outputFile, graph, result);
+  std::cout << "done! " << std::endl;
+
+  return 0;
+}

gtsam/geometry/Unit3.cpp

@@ -151,9 +151,9 @@ Vector Unit3::localCoordinates(const Unit3& y) const {
   double dot = p.dot(q);
 
   // Check for special cases
-  if (std::abs(dot - 1.0) < 1e-20)
+  if (std::abs(dot - 1.0) < 1e-16)
     return (Vector(2) << 0, 0);
-  else if (std::abs(dot + 1.0) < 1e-20)
+  else if (std::abs(dot + 1.0) < 1e-16)
     return (Vector(2) << M_PI, 0);
   else {
     // no special case

gtsam/nonlinear/DoglegOptimizerImpl.h

@@ -191,7 +191,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
       if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)
         stay = false;   // If not searching, just return with the new Delta
       else if(mode == SEARCH_EACH_ITERATION) {
-        if(newDelta == Delta || lastAction == DECREASED_DELTA)
+        if(fabs(newDelta - Delta) < 1e-15 || lastAction == DECREASED_DELTA)
           stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region
         else {
           stay = true;  // Searching and increased Delta, so try again to increase Delta

gtsam/slam/dataset.cpp

@@ -537,6 +537,118 @@ bool readBundler(const string& filename, SfM_data &data)
   return true;
 }
 
+/* ************************************************************************* */
+bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial,
+    const kernelFunctionType kernelFunction){
+
+  ifstream is(g2oFile.c_str());
+  if (!is){
+    throw std::invalid_argument("File not found!");
+    return false;
+  }
+
+  std::cout << "Reading g2o file: " << g2oFile << std::endl;
+  // READ INITIAL GUESS FROM G2O FILE
+  string tag;
+  while (is) {
+    if(! (is >> tag))
+      break;
+
+    if (tag == "VERTEX_SE2") {
+      int id;
+      double x, y, yaw;
+      is >> id >> x >> y >> yaw;
+      initial.insert(id, Pose2(x, y, yaw));
+    }
+    is.ignore(LINESIZE, '\n');
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  is.seekg(0, ios::beg);
+  // initial.print("initial guess");
+
+  // READ MEASUREMENTS FROM G2O FILE
+  while (is) {
+    if(! (is >> tag))
+      break;
+
+    if (tag == "EDGE_SE2") {
+      int id1, id2;
+      double x, y, yaw;
+      double I11, I12, I13, I22, I23, I33;
+
+      is >> id1 >> id2 >> x >> y >> yaw;
+      is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33;
+      Pose2 l1Xl2(x, y, yaw);
+      noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33));
+
+      switch (kernelFunction) {
+      {case QUADRATIC:
+        NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
+        graph.add(factor);
+        break;}
+      {case HUBER:
+        NonlinearFactor::shared_ptr huberFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
+            noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model)));
+        graph.add(huberFactor);
+        break;}
+      {case TUKEY:
+        NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
+            noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model)));
+        graph.add(tukeyFactor);
+        break;}
+      }
+    }
+    is.ignore(LINESIZE, '\n');
+  }
+  // Output which kernel is used
+  switch (kernelFunction) {
+  case QUADRATIC:
+      std::cout << "Robust kernel: None" << std::endl; break;
+  case HUBER:
+    std::cout << "Robust kernel: Huber" << std::endl; break;
+  case TUKEY:
+    std::cout << "Robust kernel: Tukey" << std::endl; break;
+  }
+  return true;
+}
+
+/* ************************************************************************* */
+bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){
+
+  fstream stream(filename.c_str(), fstream::out);
+
+  // save poses
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate)
+  {
+    const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
+    stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
+        << pose.y() << " " << pose.theta() << endl;
+  }
+
+  // save edges
+  BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph)
+  {
+    boost::shared_ptr<BetweenFactor<Pose2> > factor =
+        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
+    if (!factor)
+      continue;
+
+    SharedNoiseModel model = factor->get_noiseModel();
+    boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
+        boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
+    if (!diagonalModel)
+      throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!");
+
+    Pose2 pose = factor->measured(); //.inverse();
+    stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
+        << pose.x() << " " << pose.y() << " " << pose.theta() << " "
+        << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
+        << diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl;
+  }
+  stream.close();
+  return true;
+}
+
 /* ************************************************************************* */
 bool readBAL(const string& filename, SfM_data &data)
 {

gtsam/slam/dataset.h

@@ -117,6 +117,25 @@ struct SfM_data
  */
 GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
 
+/**
+ * @brief This function parses a g2o file and stores the measurements into a
+ * NonlinearFactorGraph and the initial guess in a Values structure
+ * @param filename The name of the g2o file
+ * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
+ * @return initial Values containing the initial guess (VERTEX_SE2)
+ */
+enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
+bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
+
+/**
+ * @brief This function writes a g2o file from
+ * NonlinearFactorGraph and a Values structure
+ * @param filename The name of the g2o file to write
+ * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
+ * @return estimate Values containing the values (VERTEX_SE2)
+ */
+bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate);
+
 /**
  * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
  * SfM_data structure