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 Pose2SLAMwSPCG.cpp
14 * @brief A 2D Pose SLAM example using the SimpleSPCGSolver.
15 * @author Yong-Dian Jian
16 * @date June 2, 2012
17 */
18
19// For an explanation of headers below, please see Pose2SLAMExample.cpp
20#include <gtsam/slam/BetweenFactor.h>
21#include <gtsam/geometry/Pose2.h>
22#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
23
24// In contrast to that example, however, we will use a PCG solver here
25#include <gtsam/linear/SubgraphSolver.h>
26
27using namespace std;
28using namespace gtsam;
29
30int main(int argc, char** argv) {
31 // 1. Create a factor graph container and add factors to it
32 NonlinearFactorGraph graph;
33
34 // 2a. Add a prior on the first pose, setting it to the origin
35 Pose2 prior(0.0, 0.0, 0.0); // prior at origin
36 auto priorNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.3, 0.3, 0.1));
37 graph.addPrior(key: 1, prior, model: priorNoise);
38
39 // 2b. Add odometry factors
40 auto odometryNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.2, 0.2, 0.1));
41 graph.emplace_shared<BetweenFactor<Pose2> >(args: 1, args: 2, args: Pose2(2.0, 0.0, M_PI_2), args&: odometryNoise);
42 graph.emplace_shared<BetweenFactor<Pose2> >(args: 2, args: 3, args: Pose2(2.0, 0.0, M_PI_2), args&: odometryNoise);
43 graph.emplace_shared<BetweenFactor<Pose2> >(args: 3, args: 4, args: Pose2(2.0, 0.0, M_PI_2), args&: odometryNoise);
44 graph.emplace_shared<BetweenFactor<Pose2> >(args: 4, args: 5, args: Pose2(2.0, 0.0, M_PI_2), args&: odometryNoise);
45
46 // 2c. Add the loop closure constraint
47 auto model = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.2, 0.2, 0.1));
48 graph.emplace_shared<BetweenFactor<Pose2> >(args: 5, args: 1, args: Pose2(0.0, 0.0, 0.0),
49 args&: model);
50 graph.print(str: "\nFactor Graph:\n"); // print
51
52 // 3. Create the data structure to hold the initialEstimate estimate to the
53 // solution
54 Values initialEstimate;
55 initialEstimate.insert(j: 1, val: Pose2(0.5, 0.0, 0.2));
56 initialEstimate.insert(j: 2, val: Pose2(2.3, 0.1, 1.1));
57 initialEstimate.insert(j: 3, val: Pose2(2.1, 1.9, 2.8));
58 initialEstimate.insert(j: 4, val: Pose2(-.3, 2.5, 4.2));
59 initialEstimate.insert(j: 5, val: Pose2(0.1, -0.7, 5.8));
60 initialEstimate.print(str: "\nInitial Estimate:\n"); // print
61
62 // 4. Single Step Optimization using Levenberg-Marquardt
63 LevenbergMarquardtParams parameters;
64 parameters.verbosity = NonlinearOptimizerParams::ERROR;
65 parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
66
67 // LM is still the outer optimization loop, but by specifying "Iterative"
68 // below We indicate that an iterative linear solver should be used. In
69 // addition, the *type* of the iterativeParams decides on the type of
70 // iterative solver, in this case the SPCG (subgraph PCG)
71 parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
72 parameters.iterativeParams = std::make_shared<SubgraphSolverParameters>();
73
74 LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
75 Values result = optimizer.optimize();
76 result.print(str: "Final Result:\n");
77 cout << "subgraph solver final error = " << graph.error(values: result) << endl;
78
79 return 0;
80}
81