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();
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  }
100  Values result = lm.optimize();
101  cout << "cost = " << graph.error(result) << endl;
102  }
103 
105  tictoc_print_();
106 
107  return 0;
108 }
timing.h
Timing utilities.
gNoiseModel
static SharedNoiseModel gNoiseModel
Definition: timeShonanFactor.cpp:40
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
main
int main(int argc, char *argv[])
Definition: timeShonanFactor.cpp:42
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
ShonanFactor.h
Main factor type in Shonan averaging, on SO(n) pairs.
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
dataset.h
utility functions for loading datasets
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gttic_
#define gttic_(label)
Definition: timing.h:245
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::tictoc_finishedIteration_
void tictoc_finishedIteration_()
Definition: timing.h:264
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
PCGSolver.h
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::ShonanFactor
Definition: ShonanFactor.h:36
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
NonlinearEquality.h
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam
traits
Definition: SFMdata.h:40
PriorFactor.h
NoiseModel.h
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
SubgraphPreconditioner.h
G
JacobiRotation< float > G
Definition: Jacobi_makeGivens.cpp:2
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
initial
Definition: testScenarioRunner.cpp:148
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
gtsam::parseVariables< Pose3 >
GTSAM_EXPORT std::map< size_t, Pose3 > parseVariables< Pose3 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:775
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:30