Shonan C++ CLI

examples/ShonanAveragingCLI.cpp

@@ -0,0 +1,125 @@
+/* ----------------------------------------------------------------------------
+
+* 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
+ *
+ */
+
+#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;
+  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")(
+      "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;
+  if (d == 2) {
+    cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
+    ShonanAveraging2 shonan(inputFile);
+    auto initial = shonan.initializeRandomly(rng);
+    auto result = shonan.run(initial);
+
+    // 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 shonan(inputFile);
+    auto initial = shonan.initializeRandomly(rng);
+    auto result = shonan.run(initial);
+
+    // 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(NonlinearFactorGraph(), poses, outputFile);
+  return 0;
+}
+
+/* ************************************************************************* */

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -302,7 +302,7 @@ TEST(ShonanAveraging3, runKlausKarcher) {
 }
 
 /* ************************************************************************* */
-TEST(ShonanAveraging2, runKlausKarcher) {
+TEST(ShonanAveraging2, noisyToyGraph) {
   // Load 2D toy example
   auto lmParams = LevenbergMarquardtParams::CeresDefaults();
   // lmParams.setVerbosityLM("SUMMARY");