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
12/**
13 * @file Pose2SLAMExampleExpressions.cpp
14 * @brief Expressions version of Pose2SLAMExample.cpp
15 * @date Oct 2, 2014
16 * @author Frank Dellaert
17 */
18
19// The two new headers that allow using our Automatic Differentiation Expression framework
20#include <gtsam/slam/expressions.h>
21#include <gtsam/nonlinear/ExpressionFactorGraph.h>
22
23// For an explanation of headers below, please see Pose2SLAMExample.cpp
24#include <gtsam/slam/BetweenFactor.h>
25#include <gtsam/geometry/Pose2.h>
26#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
27#include <gtsam/nonlinear/Marginals.h>
28
29using namespace std;
30using namespace gtsam;
31
32int main(int argc, char** argv) {
33 // 1. Create a factor graph container and add factors to it
34 ExpressionFactorGraph graph;
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(sigmas: Vector3(0.3, 0.3, 0.1));
41 graph.addExpressionFactor(h: x1, z: Pose2(0, 0, 0), R: priorNoise);
42
43 // For simplicity, we use the same noise model for odometry and loop closures
44 auto model = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.2, 0.2, 0.1));
45
46 // 2b. Add odometry factors
47 graph.addExpressionFactor(h: between(t1: x1, t2: x2), z: Pose2(2, 0, 0), R: model);
48 graph.addExpressionFactor(h: between(t1: x2, t2: x3), z: Pose2(2, 0, M_PI_2), R: model);
49 graph.addExpressionFactor(h: between(t1: x3, t2: x4), z: Pose2(2, 0, M_PI_2), R: model);
50 graph.addExpressionFactor(h: between(t1: x4, t2: x5), z: Pose2(2, 0, M_PI_2), R: model);
51
52 // 2c. Add the loop closure constraint
53 graph.addExpressionFactor(h: between(t1: x5, t2: x2), z: Pose2(2, 0, M_PI_2), R: model);
54 graph.print(str: "\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(j: 1, val: Pose2(0.5, 0.0, 0.2));
61 initialEstimate.insert(j: 2, val: Pose2(2.3, 0.1, -0.2));
62 initialEstimate.insert(j: 3, val: Pose2(4.1, 0.1, M_PI_2));
63 initialEstimate.insert(j: 4, val: Pose2(4.0, 2.0, M_PI));
64 initialEstimate.insert(j: 5, val: Pose2(2.1, 2.1, -M_PI_2));
65 initialEstimate.print(str: "\nInitial Estimate:\n"); // print
66
67 // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
68 GaussNewtonParams parameters;
69 parameters.relativeErrorTol = 1e-5;
70 parameters.maxIterations = 100;
71 GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
72 Values result = optimizer.optimize();
73 result.print(str: "Final Result:\n");
74
75 // 5. Calculate and print marginal covariances for all variables
76 cout.precision(prec: 3);
77 Marginals marginals(graph, result);
78 cout << "x1 covariance:\n" << marginals.marginalCovariance(variable: 1) << endl;
79 cout << "x2 covariance:\n" << marginals.marginalCovariance(variable: 2) << endl;
80 cout << "x3 covariance:\n" << marginals.marginalCovariance(variable: 3) << endl;
81 cout << "x4 covariance:\n" << marginals.marginalCovariance(variable: 4) << endl;
82 cout << "x5 covariance:\n" << marginals.marginalCovariance(variable: 5) << endl;
83
84 return 0;
85}
86