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