timeSFMBAL.h
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 #pragma once
20 
21 #include <gtsam/sfm/SfmData.h>
22 #include <gtsam/slam/dataset.h>
25 #include <gtsam/nonlinear/Values.h>
28 #include <gtsam/inference/Symbol.h>
29 #include <gtsam/base/timing.h>
30 
31 #include <string>
32 #include <vector>
33 
34 using namespace std;
35 using namespace gtsam;
39 
40 static bool gUseSchur = true;
41 static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
42 
43 // parse options and read BAL file
44 SfmData preamble(int argc, char* argv[]) {
45  // primitive argument parsing:
46  if (argc > 2) {
47  if (strcmp(argv[1], "--colamd"))
48  gUseSchur = false;
49  else
50  throw runtime_error("Usage: timeSFMBALxxx [--colamd] [BALfile]");
51  }
52 
53  // Load BAL file
54  SfmData db;
55  string filename;
56  if (argc > 1)
57  filename = argv[argc - 1];
58  else
59  filename = findExampleDataFile("dubrovnik-16-22106-pre");
60  return SfmData::FromBalFile(filename);
61 }
62 
63 // Create ordering and optimize
65  const Values& initial, bool separateCalibration = false) {
66  using symbol_shorthand::P;
67 
68  // Set parameters to be similar to ceres
70  LevenbergMarquardtParams::SetCeresDefaults(&params);
71 // params.setLinearSolverType("SEQUENTIAL_CHOLESKY");
72 // params.setVerbosityLM("SUMMARY");
73 
74  if (gUseSchur) {
75  // Create Schur-complement ordering
77  for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(P(j));
78  for (size_t i = 0; i < db.numberCameras(); i++) {
79  ordering.push_back(C(i));
80  if (separateCalibration) ordering.push_back(K(i));
81  }
82  params.setOrdering(ordering);
83  }
84 
85  // Optimize
86  {
89  Values result = lm.optimize();
90  }
91 
93  tictoc_print_();
94 
95  return 0;
96 }
timing.h
Timing utilities.
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Ordering.h
Variable ordering for the elimination algorithm.
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::SfmData
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
gNoiseModel
static SharedNoiseModel gNoiseModel
Definition: timeSFMBAL.h:41
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::tictoc_finishedIteration_
void tictoc_finishedIteration_()
Definition: timing.h:264
relicense.filename
filename
Definition: relicense.py:57
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Symbol.h
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
preamble
SfmData preamble(int argc, char *argv[])
Definition: timeSFMBAL.h:44
ordering
static enum @1096 ordering
gtsam::SfmData::numberCameras
size_t numberCameras() const
The number of cameras.
Definition: SfmData.h:77
SfmData.h
Data structure for dealing with Structure from Motion data.
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
NoiseModel.h
K
#define K
Definition: igam.h:8
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
gUseSchur
static bool gUseSchur
Definition: timeSFMBAL.h:40
std
Definition: BFloat16.h:88
P
static double P[]
Definition: ellpe.c:68
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
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::SfmData::numberTracks
size_t numberTracks() const
The number of reconstructed 3D points.
Definition: SfmData.h:74
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.


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