timeShonanFactor.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  * -------------------------------------------------------------------------- */
11 
19 #include <gtsam/base/timing.h>
20 #include <gtsam/geometry/Pose3.h>
22 #include <gtsam/linear/PCGSolver.h>
27 #include <gtsam/nonlinear/Values.h>
28 #include <gtsam/slam/PriorFactor.h>
29 #include <gtsam/slam/dataset.h>
30 #include <gtsam/sfm/ShonanFactor.h>
31 
32 #include <iostream>
33 #include <random>
34 #include <string>
35 #include <vector>
36 
37 using namespace std;
38 using namespace gtsam;
39 
40 static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
41 
42 int main(int argc, char* argv[]) {
43  // primitive argument parsing:
44  if (argc > 3) {
45  throw runtime_error("Usage: timeShonanFactor [g2oFile]");
46  }
47 
48  string g2oFile;
49  try {
50  if (argc > 1)
51  g2oFile = argv[argc - 1];
52  else
53  g2oFile = findExampleDataFile("sphere_smallnoise.graph");
54  } catch (const exception& e) {
55  cerr << e.what() << '\n';
56  exit(1);
57  }
58 
59  // Read G2O file
60  const auto measurements = parseMeasurements<Rot3>(g2oFile);
61  const auto poses = parseVariables<Pose3>(g2oFile);
62 
63  // Build graph
65  // graph.add(NonlinearEquality<SOn>(0, SOn::Identity(4)));
66  auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
67  graph.add(PriorFactor<SOn>(0, SOn::Identity(4), priorModel));
68  auto G = std::make_shared<Matrix>(SOn::VectorizedGenerators(4));
69  for (const auto &m : measurements) {
70  const auto &keys = m.keys();
71  const Rot3 &Rij = m.measured();
72  const auto &model = m.noiseModel();
73  graph.emplace_shared<ShonanFactor3>(
74  keys[0], keys[1], Rij, 4, model, G);
75  }
76 
77  std::mt19937 rng(42);
78 
79  // Set parameters to be similar to ceres
81  LevenbergMarquardtParams::SetCeresDefaults(&params);
82  params.setLinearSolverType("MULTIFRONTAL_QR");
83  // params.setVerbosityLM("SUMMARY");
84  // params.linearSolverType = LevenbergMarquardtParams::Iterative;
85  // auto pcg = std::make_shared<PCGSolverParameters>();
86  // pcg->preconditioner_ =
87  // std::make_shared<SubgraphPreconditionerParameters>();
88  // std::make_shared<BlockJacobiPreconditionerParameters>();
89  // params.iterativeParams = pcg;
90 
91  // Optimize
92  for (size_t i = 0; i < 100; i++) {
95  initial.insert(0, SOn::Identity(4));
96  for (size_t j = 1; j < poses.size(); j++) {
97  initial.insert(j, SOn::Random(rng, 4));
98  }
99  LevenbergMarquardtOptimizer lm(graph, initial, params);
100  Values result = lm.optimize();
101  cout << "cost = " << graph.error(result) << endl;
102  }
103 
105  tictoc_print_();
106 
107  return 0;
108 }
Matrix3f m
Main factor type in Shonan averaging, on SO(n) pairs.
void setLinearSolverType(const std::string &solver)
virtual const Values & optimize()
#define gttic_(label)
Definition: timing.h:245
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
JacobiRotation< float > G
noiseModel::Diagonal::shared_ptr model
static std::mt19937 rng
Values initial
Definition: BFloat16.h:88
GTSAM_EXPORT std::map< size_t, Pose3 > parseVariables< Pose3 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:775
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static const SmartProjectionParams params
int main(int argc, char *argv[])
Values result
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
void tictoc_print_()
Definition: timing.h:268
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static SharedNoiseModel gNoiseModel
traits
Definition: chartTesting.h:28
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
void insert(Key j, const Value &val)
Definition: Values.cpp:155
void tictoc_finishedIteration_()
Definition: timing.h:264
const KeyVector keys
3D Pose
utility functions for loading datasets
std::ptrdiff_t j
Timing utilities.
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:20