From 45d1c4f0ed2a73b685ae3a174d6d241fb7e2a565 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 05:21:32 +0000 Subject: [PATCH] Removed SLAM namespaces from OdometryExample --- examples/OdometryExample.cpp | 117 ++++++++++++++++++++++------------- 1 file changed, 73 insertions(+), 44 deletions(-) diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 36ad5f228..f0874bedb 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -15,62 +15,91 @@ * @author Frank Dellaert */ -// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined -#include - -// include this for marginals -#include - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::noiseModel; - /** * 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 */ + +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions +#include +#include + +// Each variable in the system (poses) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use symbols +#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 the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) - pose2SLAM::Graph graph; + // Create a factor graph container + NonlinearFactorGraph graph; - // add a Gaussian prior on pose x_1 - Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPosePrior(1, priorMean, priorNoise); // add directly to 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 prior(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(Symbol('x', 1), prior, priorNoise)); - // add two odometry factors - Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.addRelativePose(1, 2, odometry, odometryNoise); - graph.addRelativePose(2, 3, odometry, odometryNoise); + // Add odometry factors + // 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(Symbol('x', 1), Symbol('x', 2), Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.add(BetweenFactor(Symbol('x', 2), Symbol('x', 3), Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.print("\nFactor Graph:\n"); // print - // print - graph.print("\nFactor graph:\n"); + // Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(Symbol('x', 1), Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(Symbol('x', 2), Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(Symbol('x', 3), Pose2(4.1, 0.1, 0.1)); + initialEstimate.print("\nInitial Estimate:\n"); // print - // create (deliberatly inaccurate) initial estimate - pose2SLAM::Values initialEstimate; - initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insertPose(2, Pose2(2.3, 0.1, -0.2)); - initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.print("\nInitial estimate:\n "); + // optimize using Levenberg-Marquardt optimization + Values result = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); + result.print("Final Result:\n"); - // optimize using Levenberg-Marquardt optimization with an ordering from colamd - pose2SLAM::Values result = graph.optimize(initialEstimate); - result.print("\nFinal result:\n "); + // Calculate and print marginal covariances for all variables + Marginals marginals(graph, result); + cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl; + cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl; + cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl; - // Query the marginals - cout.precision(2); - Marginals marginals = graph.marginals(result); - cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl; - cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl; - cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl; - - return 0; + return 0; } -