removed graphWithPrior from all examples while keeping functionality the same

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/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

@@ -21,8 +21,8 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-#include <fstream>
 #include <gtsam/nonlinear/Marginals.h>
+#include <fstream>
 
 using namespace std;
 using namespace gtsam;
@@ -42,21 +42,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;
 
@@ -68,7 +67,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;
   }
 

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;