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 Pose2SLAMExample_graphviz.cpp
14 * @brief Save factor graph as graphviz dot file
15 * @date Sept 6, 2013
16 * @author Frank Dellaert
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#include <fstream>
24
25using namespace std;
26using namespace gtsam;
27
28int main(int argc, char** argv) {
29 // 1. Create a factor graph container and add factors to it
30 NonlinearFactorGraph graph;
31
32 // 2a. Add a prior on the first pose, setting it to the origin
33 auto priorNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.3, 0.3, 0.1));
34 graph.addPrior(key: 1, prior: Pose2(0, 0, 0), model: priorNoise);
35
36 // For simplicity, we will use the same noise model for odometry and loop
37 // closures
38 auto model = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.2, 0.2, 0.1));
39
40 // 2b. Add odometry factors
41 graph.emplace_shared<BetweenFactor<Pose2> >(args: 1, args: 2, args: Pose2(2, 0, 0), args&: model);
42 graph.emplace_shared<BetweenFactor<Pose2> >(args: 2, args: 3, args: Pose2(2, 0, M_PI_2), args&: model);
43 graph.emplace_shared<BetweenFactor<Pose2> >(args: 3, args: 4, args: Pose2(2, 0, M_PI_2), args&: model);
44 graph.emplace_shared<BetweenFactor<Pose2> >(args: 4, args: 5, args: Pose2(2, 0, M_PI_2), args&: model);
45
46 // 2c. Add the loop closure constraint
47 graph.emplace_shared<BetweenFactor<Pose2> >(args: 5, args: 2, args: Pose2(2, 0, M_PI_2), args&: model);
48
49 // 3. Create the data structure to hold the initial estimate to the solution
50 // For illustrative purposes, these have been deliberately set to incorrect values
51 Values initial;
52 initial.insert(j: 1, val: Pose2(0.5, 0.0, 0.2));
53 initial.insert(j: 2, val: Pose2(2.3, 0.1, -0.2));
54 initial.insert(j: 3, val: Pose2(4.1, 0.1, M_PI_2));
55 initial.insert(j: 4, val: Pose2(4.0, 2.0, M_PI));
56 initial.insert(j: 5, val: Pose2(2.1, 2.1, -M_PI_2));
57
58 // Single Step Optimization using Levenberg-Marquardt
59 Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
60
61 // save factor graph as graphviz dot file
62 // Render to PDF using "fdp Pose2SLAMExample.dot -Tpdf > graph.pdf"
63 graph.saveGraph(filename: "Pose2SLAMExample.dot", values: result);
64
65 // Also print out to console
66 graph.dot(os&: cout, values: result);
67
68 return 0;
69}
70