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
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
|
/* ----------------------------------------------------------------------------
* 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 ShonanAveragingCLI.cpp
* @brief Run Shonan Rotation Averaging Algorithm on a file or example dataset
* @author Frank Dellaert
* @date August, 2020
*
* Example usage:
*
* Running without arguments will run on tiny 3D example pose3example-grid
* ./ShonanAveragingCLI
*
* Read 2D dataset w10000 (in examples/data) and output to w10000-rotations.g2o
* ./ShonanAveragingCLI -d 2 -n w10000 -o w10000-rotations.g2o
*
* Read 3D dataset sphere25000.txt and output to shonan.g2o (default)
* ./ShonanAveragingCLI -i spere2500.txt
*
* If you prefer using a robust Huber loss, you can add the option "-h true",
* for instance
* ./ShonanAveragingCLI -i spere2500.txt -h true
*/
#include <gtsam/base/timing.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/slam/InitializePose.h>
#include <gtsam/slam/dataset.h>
#include <boost/program_options.hpp>
using namespace std;
using namespace gtsam;
namespace po = boost::program_options;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
string datasetName;
string inputFile;
string outputFile;
int d, seed, pMin;
bool useHuberLoss;
po::options_description desc(
"Shonan Rotation Averaging CLI reads a *pose* graph, extracts the "
"rotation constraints, and runs the Shonan algorithm.");
desc.add_options()("help", "Print help message")(
"named_dataset,n",
po::value<string>(&datasetName)->default_value("pose3example-grid"),
"Find and read frome example dataset file")(
"input_file,i", po::value<string>(&inputFile)->default_value(""),
"Read pose constraints graph from the specified file")(
"output_file,o",
po::value<string>(&outputFile)->default_value("shonan.g2o"),
"Write solution to the specified file")(
"dimension,d", po::value<int>(&d)->default_value(3),
"Optimize over 2D or 3D rotations")(
"useHuberLoss,h", po::value<bool>(&useHuberLoss)->default_value(false),
"set True to use Huber loss")("pMin,p",
po::value<int>(&pMin)->default_value(3),
"set to use desired rank pMin")(
"seed,s", po::value<int>(&seed)->default_value(42),
"Random seed for initial estimate");
po::variables_map vm;
po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
po::notify(vm);
if (vm.count("help")) {
cout << desc << "\n";
return 1;
}
// Get input file
if (inputFile.empty()) {
if (datasetName.empty()) {
cout << "You must either specify a named dataset or an input file\n"
<< desc << endl;
return 1;
}
inputFile = findExampleDataFile(datasetName);
}
// Seed random number generator
static std::mt19937 rng(seed);
NonlinearFactorGraph::shared_ptr inputGraph;
Values::shared_ptr posesInFile;
Values poses;
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
if (d == 2) {
cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
ShonanAveraging2::Parameters parameters(lmParams);
parameters.setUseHuber(useHuberLoss);
ShonanAveraging2 shonan(inputFile, parameters);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial, pMin);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load2D(inputFile);
auto priorModel = noiseModel::Unit::Create(3);
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
cout << "recovering 2D translations" << endl;
auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
poses = initialize::computePoses<Pose2>(result.first, &poseGraph);
} else if (d == 3) {
cout << "Running Shonan averaging for SO(3) on " << inputFile << endl;
ShonanAveraging3::Parameters parameters(lmParams);
parameters.setUseHuber(useHuberLoss);
ShonanAveraging3 shonan(inputFile, parameters);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial, pMin);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load3D(inputFile);
auto priorModel = noiseModel::Unit::Create(6);
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
cout << "recovering 3D translations" << endl;
auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
poses = initialize::computePoses<Pose3>(result.first, &poseGraph);
} else {
cout << "Can only run SO(2) or SO(3) averaging\n" << desc << endl;
return 1;
}
cout << "Writing result to " << outputFile << endl;
writeG2o(*inputGraph, poses, outputFile);
return 0;
}
/* ************************************************************************* */
|
