From b925142b987af501fb86d738079e6491940f5e37 Mon Sep 17 00:00:00 2001 From: Martin Brossard Date: Fri, 2 Aug 2019 17:13:02 +0200 Subject: [PATCH 1/3] Create pose3Localizationexample.txt pose3example.txt without loop closure --- examples/Data/pose3Localizationexample.txt | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 examples/Data/pose3Localizationexample.txt diff --git a/examples/Data/pose3Localizationexample.txt b/examples/Data/pose3Localizationexample.txt new file mode 100644 index 0000000000..a35005aa2b --- /dev/null +++ b/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 From 3d7ce45de66bb401cdd7c124cf0b4ef0fd228a57 Mon Sep 17 00:00:00 2001 From: Martin Brossard Date: Fri, 2 Aug 2019 17:14:31 +0200 Subject: [PATCH 2/3] Create Pose3Localization.cpp Pose3Example_g2o with marginal computation. --- examples/Pose3Localization.cpp | 83 ++++++++++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 examples/Pose3Localization.cpp diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp new file mode 100644 index 0000000000..becb9530cb --- /dev/null +++ b/examples/Pose3Localization.cpp @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include +#include +#include + +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 + 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)); + 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); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + std::cout << "initial error=" <error(*initial)<< std::endl; + std::cout << "final error=" <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; + writeG2o(*graph, 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*>(&key_value.value); + if (!p) continue; + std::cout << marginals.marginalCovariance(key_value.key) << endl; + } + return 0; +} From 4197fa3c54751a32bcaa4dabaee9955e476ea28b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Mar 2020 19:11:44 -0400 Subject: [PATCH 3/3] 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 3aee7daff3..83d543310a 100644 --- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py @@ -53,16 +53,15 @@ def vector3(x, y, z): 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 38c5db275a..25686c762a 100644 --- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py @@ -43,18 +43,17 @@ def vector6(x, y, z, a, b, c): 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 35f9884e13..a38dfafa60 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 b83dfa1dba..d6164450bb 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 becb9530cb..05a04b3532 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 5066222e59..25297806ec 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 e90d014c06..9726f467c1 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 10960cf316..0001508460 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;