From 9c2dcfb70c57e520088834220042c144600b19d3 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 17 Nov 2014 12:06:59 -0500 Subject: [PATCH] Slim down example to remove verbosity, added explanation on orderingType --- examples/METISOrderingExample.cpp | 47 +++---------------------------- 1 file changed, 4 insertions(+), 43 deletions(-) diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 6b54b8d70..1fae4c0b9 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -17,68 +17,34 @@ */ /** - * Example of a simple 2D localization example - * - Robot poses are facing along the X axis (horizontal, to the right in 2D) - * - The robot moves 2 meters each step - * - We have full odometry between poses + * Example of a simple 2D localization example optimized using METIS ordering + * - For more details on the full optimization pipeline, see OdometryExample.cpp */ -// We will use Pose2 variables (x, y, theta) to represent the robot positions #include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Between factors for the relative motion described by odometry measurements. -// Also, we will initialize the robot at the origin using a Prior factor. #include #include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. #include - -// Finally, once all of the factors have been added to our factor graph, we will want to -// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the -// Levenberg-Marquardt solver #include - -// Once the optimized values have been calculated, we can also calculate the marginal covariance -// of desired variables #include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. #include using namespace std; using namespace gtsam; int main(int argc, char** argv) { - - // Create an empty nonlinear factor graph NonlinearFactorGraph graph; - // Add a prior on the first pose, setting it to the origin - // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); graph.add(PriorFactor(1, priorMean, priorNoise)); - // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); - // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); - // Create odometry (Between) factors between consecutive poses graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); graph.print("\nFactor Graph:\n"); // print - // Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values Values initial; initial.insert(1, Pose2(0.5, 0.0, 0.2)); initial.insert(2, Pose2(2.3, 0.1, -0.2)); @@ -87,17 +53,12 @@ int main(int argc, char** argv) { // optimize using Levenberg-Marquardt optimization LevenbergMarquardtParams params; + // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType" + // By default this parameter is set to OrderingType::COLAMD params.orderingType = OrderingType::METIS; LevenbergMarquardtOptimizer optimizer(graph, initial, params); Values result = optimizer.optimize(); result.print("Final Result:\n"); - // Calculate and print marginal covariances for all variables - cout.precision(2); - Marginals marginals(graph, result); - cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl; - cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl; - cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl; - return 0; } \ No newline at end of file