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_initializePose3.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_changeKeys input.g2o rewritted.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 <fstream>
23
24using namespace std;
25using namespace gtsam;
26
27int main(const int argc, const char *argv[]) {
28
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 bool add = false;
42 Key firstKey = 8646911284551352320;
43
44 std::cout << "Using reference key: " << firstKey << std::endl;
45 if(add)
46 std::cout << "adding key " << std::endl;
47 else
48 std::cout << "subtracting key " << std::endl;
49
50
51 if (argc < 3) {
52 std::cout << "Please provide output file to write " << std::endl;
53 } else {
54 const string inputFileRewritten = argv[2];
55 std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
56 // Additional: rewrite input with simplified keys 0,1,...
57 Values simpleInitial;
58 for (const auto k : initial->keys()) {
59 Key key;
60 if (add)
61 key = k + firstKey;
62 else
63 key = k - firstKey;
64
65 simpleInitial.insert(j: key, val: initial->at(j: k));
66 }
67 NonlinearFactorGraph simpleGraph;
68 for(const std::shared_ptr<NonlinearFactor>& factor: *graph) {
69 std::shared_ptr<BetweenFactor<Pose3> > pose3Between =
70 std::dynamic_pointer_cast<BetweenFactor<Pose3> >(r: factor);
71 if (pose3Between){
72 Key key1, key2;
73 if(add){
74 key1 = pose3Between->key<1>() + firstKey;
75 key2 = pose3Between->key<2>() + firstKey;
76 }else{
77 key1 = pose3Between->key<1>() - firstKey;
78 key2 = pose3Between->key<2>() - firstKey;
79 }
80 NonlinearFactor::shared_ptr simpleFactor(
81 new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));
82 simpleGraph.add(factor: simpleFactor);
83 }
84 }
85 writeG2o(graph: simpleGraph, estimate: simpleInitial, filename: inputFileRewritten);
86 }
87 return 0;
88}
89