ShonanAveragingCLI.cpp
Go to the documentation of this file.
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 
33 #include <gtsam/base/timing.h>
36 #include <gtsam/slam/dataset.h>
37 
38 #include <boost/program_options.hpp>
39 
40 using namespace std;
41 using namespace gtsam;
42 namespace po = boost::program_options;
43 
44 /* ************************************************************************* */
45 int 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>(&datasetName)->default_value("pose3example-grid"),
57  "Find and read frome example dataset file")(
58  "input_file,i", po::value<string>(&inputFile)->default_value(""),
59  "Read pose constraints graph from the specified file")(
60  "output_file,o",
61  po::value<string>(&outputFile)->default_value("shonan.g2o"),
62  "Write solution to the specified file")(
63  "dimension,d", po::value<int>(&d)->default_value(3),
64  "Optimize over 2D or 3D rotations")(
65  "useHuberLoss,h", po::value<bool>(&useHuberLoss)->default_value(false),
66  "set True to use Huber loss")("pMin,p",
67  po::value<int>(&pMin)->default_value(3),
68  "set to use desired rank pMin")(
69  "seed,s", po::value<int>(&seed)->default_value(42),
70  "Random seed for initial estimate");
71  po::variables_map vm;
72  po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
73  po::notify(vm);
74 
75  if (vm.count("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(datasetName);
88  }
89 
90  // Seed random number generator
91  static std::mt19937 rng(seed);
92 
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(initial, pMin);
104 
105  // Parse file again to set up translation problem, adding a prior
106  std::tie(inputGraph, posesInFile) = load2D(inputFile);
107  auto priorModel = noiseModel::Unit::Create(3);
108  inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
109 
110  cout << "recovering 2D translations" << endl;
111  auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
112  poses = initialize::computePoses<Pose2>(result.first, &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(initial, pMin);
120 
121  // Parse file again to set up translation problem, adding a prior
122  std::tie(inputGraph, posesInFile) = load3D(inputFile);
123  auto priorModel = noiseModel::Unit::Create(6);
124  inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
125 
126  cout << "recovering 3D translations" << endl;
127  auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
128  poses = initialize::computePoses<Pose3>(result.first, &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(*inputGraph, poses, outputFile);
135  return 0;
136 }
137 
138 /* ************************************************************************* */
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure. ...
Definition: dataset.cpp:636
Values initializeRandomly(std::mt19937 &rng) const
static std::mt19937 rng
std::pair< Values, double > run(const Values &initialEstimate, size_t pMin=d, size_t pMax=10) const
common code between lago.* (2D) and InitializePose3.* (3D)
Definition: BFloat16.h:88
int main(int argc, char *argv[])
string datasetName
string inputFile
Values result
idx_t idx_t idx_t idx_t idx_t idx_t idx_t real_t real_t idx_t * options
GraphAndValues load3D(const std::string &filename)
Load TORO 3D Graph.
Definition: dataset.cpp:922
GraphAndValues load2D(const std::string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Definition: dataset.cpp:505
LevenbergMarquardtParams lmParams
static ConjugateGradientParameters parameters
string outputFile
traits
Definition: chartTesting.h:28
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
std::shared_ptr< This > shared_ptr
utility functions for loading datasets
Timing utilities.
Shonan Averaging algorithm.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:42