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 Pose3Localization.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 ./Pose3Localization 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 <gtsam/nonlinear/Marginals.h>
24#include <fstream>
25
26using namespace std;
27using namespace gtsam;
28
29int main(const int argc, const char* argv[]) {
30 // Read graph from file
31 string g2oFile;
32 if (argc < 2)
33 g2oFile = findExampleDataFile(name: "pose3Localizationexample.txt");
34 else
35 g2oFile = argv[1];
36
37 NonlinearFactorGraph::shared_ptr graph;
38 Values::shared_ptr initial;
39 bool is3D = true;
40 std::tie(args&: graph, args&: initial) = readG2o(g2oFile, is3D);
41
42 // Add prior on the first key
43 auto priorModel = noiseModel::Diagonal::Variances(
44 variances: (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
45 Key firstKey = 0;
46 for (const auto key : initial->keys()) {
47 std::cout << "Adding prior to g2o file " << std::endl;
48 firstKey = key;
49 graph->addPrior(key: firstKey, prior: Pose3(), model: priorModel);
50 break;
51 }
52
53 std::cout << "Optimizing the factor graph" << std::endl;
54 GaussNewtonParams params;
55 params.setVerbosity("TERMINATION"); // show info about stopping conditions
56 GaussNewtonOptimizer optimizer(*graph, *initial, params);
57 Values result = optimizer.optimize();
58 std::cout << "Optimization complete" << std::endl;
59
60 std::cout << "initial error=" << graph->error(values: *initial) << std::endl;
61 std::cout << "final error=" << graph->error(values: result) << std::endl;
62
63 if (argc < 3) {
64 result.print(str: "result");
65 } else {
66 const string outputFile = argv[2];
67 std::cout << "Writing results to file: " << outputFile << std::endl;
68 NonlinearFactorGraph::shared_ptr graphNoKernel;
69 Values::shared_ptr initial2;
70 std::tie(args&: graphNoKernel, args&: initial2) = readG2o(g2oFile);
71 writeG2o(graph: *graphNoKernel, estimate: result, filename: outputFile);
72 std::cout << "done! " << std::endl;
73 }
74
75 // Calculate and print marginal covariances for all variables
76 Marginals marginals(*graph, result);
77 for (const auto& [key, pose] : result.extract<Pose3>()) {
78 std::cout << marginals.marginalCovariance(variable: key) << endl;
79 }
80 return 0;
81}
82