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_lago.cpp
14 * @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem
15 * using LAGO (Linear Approximation for Graph Optimization). See class lago.h
16 * Output is written on a file, in g2o format
17 * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o
18 * @date May 15, 2014
19 * @author Luca Carlone
20 */
21
22#include <gtsam/slam/lago.h>
23#include <gtsam/slam/dataset.h>
24#include <gtsam/geometry/Pose2.h>
25#include <fstream>
26
27using namespace std;
28using namespace gtsam;
29
30int main(const int argc, const char *argv[]) {
31 // Read graph from file
32 string g2oFile;
33 if (argc < 2)
34 g2oFile = findExampleDataFile(name: "noisyToyGraph.txt");
35 else
36 g2oFile = argv[1];
37
38 NonlinearFactorGraph::shared_ptr graph;
39 Values::shared_ptr initial;
40 std::tie(args&: graph, args&: initial) = readG2o(g2oFile);
41
42 // Add prior on the pose having index (key) = 0
43 auto priorModel = noiseModel::Diagonal::Variances(variances: Vector3(1e-6, 1e-6, 1e-8));
44 graph->addPrior(key: 0, prior: Pose2(), model: priorModel);
45 graph->print();
46
47 std::cout << "Computing LAGO estimate" << std::endl;
48 Values estimateLago = lago::initialize(graph: *graph);
49 std::cout << "done!" << std::endl;
50
51 if (argc < 3) {
52 estimateLago.print(str: "estimateLago");
53 } else {
54 const string outputFile = argv[2];
55 std::cout << "Writing results to file: " << outputFile << std::endl;
56 NonlinearFactorGraph::shared_ptr graphNoKernel;
57 Values::shared_ptr initial2;
58 std::tie(args&: graphNoKernel, args&: initial2) = readG2o(g2oFile);
59 writeG2o(graph: *graphNoKernel, estimate: estimateLago, filename: outputFile);
60 std::cout << "done! " << std::endl;
61 }
62
63 return 0;
64}
65