From 4197fa3c54751a32bcaa4dabaee9955e476ea28b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Mar 2020 19:11:44 -0400 Subject: [PATCH] removed graphWithPrior from all examples while keeping functionality the same --- cython/gtsam/examples/Pose2SLAMExample_g2o.py | 5 ++--- cython/gtsam/examples/Pose3SLAMExample_g2o.py | 9 ++++----- examples/Pose2SLAMExample_g2o.cpp | 5 ++--- examples/Pose2SLAMExample_lago.cpp | 12 +++++++----- examples/Pose3Localization.cpp | 12 +++++++----- examples/Pose3SLAMExample_g2o.cpp | 10 ++++++---- examples/Pose3SLAMExample_initializePose3Chordal.cpp | 10 ++++++---- .../Pose3SLAMExample_initializePose3Gradient.cpp | 10 ++++++---- 8 files changed, 40 insertions(+), 33 deletions(-) diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py index 3aee7daff..83d543310 100644 --- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/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() diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py index 38c5db275..25686c762 100644 --- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/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("final error = ", graphWithPrior.error(result)) +print("initial error = ", graph.error(initial)) +print("final error = ", graph.error(result)) if args.output is None: print("Final Result:\n{}".format(result)) diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 35f9884e1..a38dfafa6 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/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(0, Pose2(), priorModel)); + graph->add(PriorFactor(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; diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index b83dfa1db..d6164450b 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/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(0, Pose2(), priorModel)); - graphWithPrior.print(); + graph->add(PriorFactor(0, Pose2(), priorModel)); + 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; } diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp index becb9530c..05a04b353 100644 --- a/examples/Pose3Localization.cpp +++ b/examples/Pose3Localization.cpp @@ -21,8 +21,8 @@ #include #include #include -#include #include +#include 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(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(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; } diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index 5066222e5..25297806e 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/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(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(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; diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index e90d014c0..9726f467c 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/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(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(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; diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 10960cf31..000150846 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/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(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(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=" <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;