examples/OdometryExample.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
25 // We will use Pose2 variables (x, y, theta) to represent the robot positions
26 #include <gtsam/geometry/Pose2.h>
27 
28 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
29 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
30 // Here we will use Between factors for the relative motion described by odometry measurements.
31 // Also, we will initialize the robot at the origin using a Prior factor.
33 
34 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
35 // are nonlinear factors, we will need a Nonlinear Factor Graph.
37 
38 // Finally, once all of the factors have been added to our factor graph, we will want to
39 // solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
40 // GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
41 // Levenberg-Marquardt solver
43 
44 // Once the optimized values have been calculated, we can also calculate the marginal covariance
45 // of desired variables
47 
48 // The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
49 // nonlinear functions around an initial linearization point, then solve the linear system
50 // to update the linearization point. This happens repeatedly until the solver converges
51 // to a consistent set of variable values. This requires us to specify an initial guess
52 // for each variable, held in a Values container.
53 #include <gtsam/nonlinear/Values.h>
54 
55 using namespace std;
56 using namespace gtsam;
57 
58 int main(int argc, char** argv) {
59  // Create an empty nonlinear factor graph
61 
62  // Add a prior on the first pose, setting it to the origin
63  // A prior factor consists of a mean and a noise model (covariance matrix)
64  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
65  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
66  graph.addPrior(1, priorMean, priorNoise);
67 
68  // Add odometry factors
69  Pose2 odometry(2.0, 0.0, 0.0);
70  // For simplicity, we will use the same noise model for each odometry factor
71  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
72  // Create odometry (Between) factors between consecutive poses
75  graph.print("\nFactor Graph:\n"); // print
76 
77  // Create the data structure to hold the initialEstimate estimate to the solution
78  // For illustrative purposes, these have been deliberately set to incorrect values
80  initial.insert(1, Pose2(0.5, 0.0, 0.2));
81  initial.insert(2, Pose2(2.3, 0.1, -0.2));
82  initial.insert(3, Pose2(4.1, 0.1, 0.1));
83  initial.print("\nInitial Estimate:\n"); // print
84 
85  // optimize using Levenberg-Marquardt optimization
87  result.print("Final Result:\n");
88 
89  // Calculate and print marginal covariances for all variables
90  cout.precision(2);
91  Marginals marginals(graph, result);
92  cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
93  cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
94  cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
95 
96  return 0;
97 }
Matrix marginalCovariance(Key variable) const
Definition: Marginals.cpp:133
virtual const Values & optimize()
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
Eigen::Vector3d Vector3
Definition: Vector.h:43
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
int main(int argc, char **argv)
Values initial
Definition: BFloat16.h:88
NonlinearFactorGraph graph
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
Values result
auto odometryNoise
Pose2 priorMean(0.0, 0.0, 0.0)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
traits
Definition: chartTesting.h:28
auto priorNoise
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
void insert(Key j, const Value &val)
Definition: Values.cpp:155
A class for computing marginals in a NonlinearFactorGraph.
Pose2 odometry(2.0, 0.0, 0.0)
2D Pose
Marginals marginals(graph, result)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:58