Merge pull request #97 from mbrossar/develop

Marginal computation of pose-graph without loop closure

cython/gtsam/examples/Pose2SLAMExample_g2o.py

@@ -53,16 +53,15 @@ graph, initial = gtsam.readG2o(g2oFile, is3D)
 assert args.kernel == "none", "Supplied kernel type is not yet implemented"
 
 # Add prior on the pose having index (key) = 0
-graphWithPrior = graph
 priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
-graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
+graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
 
 params = gtsam.GaussNewtonParams()
 params.setVerbosity("Termination")
 params.setMaxIterations(maxIterations)
 # parameters.setRelativeErrorTol(1e-5)
 # Create the optimizer ...
-optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
+optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
 # ... and optimize
 result = optimizer.optimize()
 

cython/gtsam/examples/Pose3SLAMExample_g2o.py

@@ -43,18 +43,17 @@ priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
                                                          1e-4, 1e-4, 1e-4))
 
 print("Adding prior to g2o file ")
-graphWithPrior = graph
 firstKey = initial.keys().at(0)
-graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
+graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
 
 params = gtsam.GaussNewtonParams()
 params.setVerbosity("Termination")  # this will show info about stopping conds
-optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
+optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
 result = optimizer.optimize()
 print("Optimization complete")
 
-print("initial error = ", graphWithPrior.error(initial))
+print("initial error = ", graph.error(initial))
-print("final error = ", graphWithPrior.error(result))
+print("final error = ", graph.error(result))
 
 if args.output is None:
     print("Final Result:\n{}".format(result))

examples/Data/pose3Localizationexample.txt

@@ -0,0 +1,9 @@
+VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
+VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
+VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
+VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
+VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
+EDGE_SE3:QUAT 0 1   1.001367 0.015390 0.004948   0.190253 0.283162 -0.392318 0.854230   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 1 2   0.523923 0.776654 0.326659   0.311512 0.656877 -0.678505 0.105373   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 2 3   0.910927 0.055169 -0.411761   0.595795 -0.561677 0.079353 0.568551   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 3 4   0.775288 0.228798 -0.596923   -0.592077 0.303380 -0.513226 0.542221   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000

examples/Pose2SLAMExample_g2o.cpp

@@ -63,10 +63,9 @@ int main(const int argc, const char *argv[]) {
   }
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
-  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
   std::cout << "Adding prior on pose 0 " << std::endl;
 
   GaussNewtonParams params;
@@ -77,7 +76,7 @@ int main(const int argc, const char *argv[]) {
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
-  GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
+  GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 

examples/Pose2SLAMExample_lago.cpp

@@ -42,14 +42,13 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
-  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
-  graphWithPrior.print();
+  graph->print();
 
   std::cout << "Computing LAGO estimate" << std::endl;
-  Values estimateLago = lago::initialize(graphWithPrior);
+  Values estimateLago = lago::initialize(*graph);
   std::cout << "done!" << std::endl;
 
   if (argc < 3) {
@@ -57,7 +56,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, estimateLago, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, estimateLago, outputFile);
     std::cout << "done! " << std::endl;
   }
 

examples/Pose3Localization.cpp

@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ * @date Aug 25, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/Marginals.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("pose3Localizationexample.txt");
+  else
+    g2oFile = argv[1];
+
+  NonlinearFactorGraph::shared_ptr graph;
+  Values::shared_ptr initial;
+  bool is3D = true;
+  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+
+  // Add prior on the first key
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+  Key firstKey = 0;
+  for(const Values::ConstKeyValuePair& key_value: *initial) {
+    std::cout << "Adding prior to g2o file " << std::endl;
+    firstKey = key_value.key;
+    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    break;
+  }
+
+  std::cout << "Optimizing the factor graph" << std::endl;
+  GaussNewtonParams params;
+  params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
+  GaussNewtonOptimizer optimizer(*graph, *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 {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile << std::endl;
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, result, outputFile);
+    std::cout << "done! " << std::endl;
+  }
+
+  // Calculate and print marginal covariances for all variables
+  Marginals marginals(*graph, result);
+  for(const auto& key_value: result) {
+    auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
+    if (!p) continue;
+    std::cout << marginals.marginalCovariance(key_value.key) << endl;
+  }
+  return 0;
+}

examples/Pose3SLAMExample_g2o.cpp

@@ -41,21 +41,20 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
     break;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
   GaussNewtonParams params;
   params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
-  GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
+  GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
@@ -67,7 +66,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;

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -41,19 +41,18 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
     break;
   }
 
   std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
-  Values initialization = InitializePose3::initialize(graphWithPrior);
+  Values initialization = InitializePose3::initialize(*graph);
   std::cout << "done!" << std::endl;
 
   if (argc < 3) {
@@ -61,7 +60,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, initialization, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, initialization, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -41,20 +41,19 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
     break;
   }
 
   std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
   bool useGradient = true;
-  Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
+  Values initialization = InitializePose3::initialize(*graph, *initial, useGradient);
   std::cout << "done!" << std::endl;
 
   std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
@@ -65,7 +64,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, initialization, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, initialization, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;