diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index f0874bedb..e420a0be9 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -27,11 +27,6 @@ #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. @@ -60,35 +55,35 @@ // for each variable, held in a Values container. #include - using namespace std; using namespace gtsam; int main(int argc, char** argv) { - // Create a factor graph container + // 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 prior(0.0, 0.0, 0.0); // prior at origin + 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(Symbol('x', 1), prior, priorNoise)); + 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(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.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 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.insert(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.print("\nInitial Estimate:\n"); // print // optimize using Levenberg-Marquardt optimization @@ -97,9 +92,9 @@ int main(int argc, char** argv) { // 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; + cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(1) << endl; + cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(2) << endl; + cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(3) << endl; return 0; }