1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
|
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3Gradient.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3Gradient input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char* argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for (const auto key : initial->keys()) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key;
graph->addPrior(firstKey, Pose3(), priorModel);
break;
}
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true;
Values initialization =
InitializePose3::initialize(*graph, *initial, useGradient);
std::cout << "done!" << std::endl;
std::cout << "initial error=" << graph->error(*initial) << std::endl;
std::cout << "initialization error=" << graph->error(initialization)
<< std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}
|
