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_g2o.cpp
14 * @brief A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph and does the
15 * optimization. Output is written on a file, in g2o format
16 * Syntax for the script is ./Pose2SLAMExample_g2o input.g2o output.g2o
17 * @date May 15, 2014
18 * @author Luca Carlone
19 */
20
21#include <gtsam/slam/dataset.h>
22#include <gtsam/geometry/Pose2.h>
23#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
24#include <fstream>
25
26using namespace std;
27using namespace gtsam;
28
29// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
30int main(const int argc, const char *argv[]) {
31 string kernelType = "none";
32 int maxIterations = 100; // default
33 string g2oFile = findExampleDataFile(name: "noisyToyGraph.txt"); // default
34
35 // Parse user's inputs
36 if (argc > 1) {
37 g2oFile = argv[1]; // input dataset filename
38 }
39 if (argc > 3) {
40 maxIterations = atoi(nptr: argv[3]); // user can specify either tukey or huber
41 }
42 if (argc > 4) {
43 kernelType = argv[4]; // user can specify either tukey or huber
44 }
45
46 // reading file and creating factor graph
47 NonlinearFactorGraph::shared_ptr graph;
48 Values::shared_ptr initial;
49 bool is3D = false;
50 if (kernelType.compare(s: "none") == 0) {
51 std::tie(args&: graph, args&: initial) = readG2o(g2oFile, is3D);
52 }
53 if (kernelType.compare(s: "huber") == 0) {
54 std::cout << "Using robust kernel: huber " << std::endl;
55 std::tie(args&: graph, args&: initial) =
56 readG2o(g2oFile, is3D, kernelFunctionType: KernelFunctionTypeHUBER);
57 }
58 if (kernelType.compare(s: "tukey") == 0) {
59 std::cout << "Using robust kernel: tukey " << std::endl;
60 std::tie(args&: graph, args&: initial) =
61 readG2o(g2oFile, is3D, kernelFunctionType: KernelFunctionTypeTUKEY);
62 }
63
64 // Add prior on the pose having index (key) = 0
65 auto priorModel = //
66 noiseModel::Diagonal::Variances(variances: Vector3(1e-6, 1e-6, 1e-8));
67 graph->addPrior(key: 0, prior: Pose2(), model: priorModel);
68 std::cout << "Adding prior on pose 0 " << std::endl;
69
70 GaussNewtonParams params;
71 params.setVerbosity("TERMINATION");
72 if (argc > 3) {
73 params.maxIterations = maxIterations;
74 std::cout << "User required to perform maximum " << params.maxIterations
75 << " iterations " << std::endl;
76 }
77
78 std::cout << "Optimizing the factor graph" << std::endl;
79 GaussNewtonOptimizer optimizer(*graph, *initial, params);
80 Values result = optimizer.optimize();
81 std::cout << "Optimization complete" << std::endl;
82
83 std::cout << "initial error=" << graph->error(values: *initial) << std::endl;
84 std::cout << "final error=" << graph->error(values: result) << std::endl;
85
86 if (argc < 3) {
87 result.print(str: "result");
88 } else {
89 const string outputFile = argv[2];
90 std::cout << "Writing results to file: " << outputFile << std::endl;
91 NonlinearFactorGraph::shared_ptr graphNoKernel;
92 Values::shared_ptr initial2;
93 std::tie(args&: graphNoKernel, args&: initial2) = readG2o(g2oFile);
94 writeG2o(graph: *graphNoKernel, estimate: result, filename: outputFile);
95 std::cout << "done! " << std::endl;
96 }
97 return 0;
98}
99