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 Pose3SLAMExample_g2o.cpp
14 * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
15 * Syntax for the script is ./Pose3SLAMExample_g2o input.g2o output.g2o
16 * @date Aug 25, 2014
17 * @author Luca Carlone
18 */
19
20#include <gtsam/slam/dataset.h>
21#include <gtsam/slam/BetweenFactor.h>
22#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
23#include <fstream>
24
25using namespace std;
26using namespace gtsam;
27
28int main(const int argc, const char* argv[]) {
29 // Read graph from file
30 string g2oFile;
31 if (argc < 2)
32 g2oFile = findExampleDataFile(name: "pose3example.txt");
33 else
34 g2oFile = argv[1];
35
36 NonlinearFactorGraph::shared_ptr graph;
37 Values::shared_ptr initial;
38 bool is3D = true;
39 std::tie(args&: graph, args&: initial) = readG2o(g2oFile, is3D);
40
41 // Add prior on the first key
42 auto priorModel = noiseModel::Diagonal::Variances(
43 variances: (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
44 Key firstKey = 0;
45 for (const auto key : initial->keys()) {
46 std::cout << "Adding prior to g2o file " << std::endl;
47 firstKey = key;
48 graph->addPrior(key: firstKey, prior: Pose3(), model: priorModel);
49 break;
50 }
51
52 std::cout << "Optimizing the factor graph" << std::endl;
53 GaussNewtonParams params;
54 params.setVerbosity("TERMINATION"); // show info about stopping conditions
55 GaussNewtonOptimizer optimizer(*graph, *initial, params);
56 Values result = optimizer.optimize();
57 std::cout << "Optimization complete" << std::endl;
58
59 std::cout << "initial error=" << graph->error(values: *initial) << std::endl;
60 std::cout << "final error=" << graph->error(values: result) << std::endl;
61
62 if (argc < 3) {
63 result.print(str: "result");
64 } else {
65 const string outputFile = argv[2];
66 std::cout << "Writing results to file: " << outputFile << std::endl;
67 NonlinearFactorGraph::shared_ptr graphNoKernel;
68 Values::shared_ptr initial2;
69 std::tie(args&: graphNoKernel, args&: initial2) = readG2o(g2oFile);
70 writeG2o(graph: *graphNoKernel, estimate: result, filename: outputFile);
71 std::cout << "done! " << std::endl;
72 }
73 return 0;
74}
75