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_graph.cpp
14 * @brief Read graph from file and perform GraphSLAM
15 * @date June 3, 2012
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 <gtsam/nonlinear/Marginals.h>
24
25// This new header allows us to read examples easily from .graph files
26#include <gtsam/slam/dataset.h>
27
28using namespace std;
29using namespace gtsam;
30
31int main (int argc, char** argv) {
32
33 // Read File, create graph and initial estimate
34 // we are in build/examples, data is in examples/Data
35 NonlinearFactorGraph::shared_ptr graph;
36 Values::shared_ptr initial;
37 SharedDiagonal model = noiseModel::Diagonal::Sigmas(sigmas: (Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
38 string graph_file = findExampleDataFile(name: "w100.graph");
39 std::tie(args&: graph, args&: initial) = load2D(filename: graph_file, model);
40 initial->print(str: "Initial estimate:\n");
41
42 // Add a Gaussian prior on first poses
43 Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
44 SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.01, 0.01, 0.01));
45 graph -> addPrior(key: 0, prior: priorMean, model: priorNoise);
46
47 // Single Step Optimization using Levenberg-Marquardt
48 Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize();
49 result.print(str: "\nFinal result:\n");
50
51 // Plot the covariance of the last pose
52 Marginals marginals(*graph, result);
53 cout.precision(prec: 2);
54 cout << "\nP3:\n" << marginals.marginalCovariance(variable: 99) << endl;
55
56 return 0;
57}
58