Pose2SLAMExampleExpressions.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 
19 // The two new headers that allow using our Automatic Differentiation Expression framework
20 #include <gtsam/slam/expressions.h>
22 
23 // For an explanation of headers below, please see Pose2SLAMExample.cpp
25 #include <gtsam/geometry/Pose2.h>
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 int main(int argc, char** argv) {
33  // 1. Create a factor graph container and add factors to it
35 
36  // Create Expressions for unknowns
37  Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
38 
39  // 2a. Add a prior on the first pose, setting it to the origin
40  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
41  graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
42 
43  // For simplicity, we use the same noise model for odometry and loop closures
44  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
45 
46  // 2b. Add odometry factors
47  graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model);
48  graph.addExpressionFactor(between(x2, x3), Pose2(2, 0, M_PI_2), model);
49  graph.addExpressionFactor(between(x3, x4), Pose2(2, 0, M_PI_2), model);
50  graph.addExpressionFactor(between(x4, x5), Pose2(2, 0, M_PI_2), model);
51 
52  // 2c. Add the loop closure constraint
53  graph.addExpressionFactor(between(x5, x2), Pose2(2, 0, M_PI_2), model);
54  graph.print("\nFactor Graph:\n"); // print
55 
56  // 3. Create the data structure to hold the initialEstimate estimate to the
57  // solution For illustrative purposes, these have been deliberately set to
58  // incorrect values
59  Values initialEstimate;
60  initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
61  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
62  initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
63  initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
64  initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
65  initialEstimate.print("\nInitial Estimate:\n"); // print
66 
67  // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
69  parameters.relativeErrorTol = 1e-5;
70  parameters.maxIterations = 100;
71  GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
72  Values result = optimizer.optimize();
73  result.print("Final Result:\n");
74 
75  // 5. Calculate and print marginal covariances for all variables
76  cout.precision(3);
77  Marginals marginals(graph, result);
78  cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
79  cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
80  cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
81  cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl;
82  cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl;
83 
84  return 0;
85 }
Matrix marginalCovariance(Key variable) const
Definition: Marginals.cpp:133
virtual const Values & optimize()
Eigen::Vector3d Vector3
Definition: Vector.h:43
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
noiseModel::Diagonal::shared_ptr model
#define M_PI
Definition: main.h:106
Definition: BFloat16.h:88
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
static string x5("x5")
NonlinearFactorGraph graph
int main(int argc, char **argv)
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
Values result
Factor graph that supports adding ExpressionFactors directly.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static ConjugateGradientParameters parameters
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
traits
Definition: chartTesting.h:28
auto priorNoise
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Pose3 x1
Definition: testPose3.cpp:663
void insert(Key j, const Value &val)
Definition: Values.cpp:155
void addExpressionFactor(const Expression< T > &h, const T &z, const SharedNoiseModel &R)
A class for computing marginals in a NonlinearFactorGraph.
2D Pose
static string x4("x4")
Marginals marginals(graph, result)
size_t maxIterations
The maximum iterations to stop iterating (default 100)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:14