SFMExample_bal.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 
18 // For an explanation of headers, see SFMExample.cpp
19 #include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
21 #include <gtsam/slam/dataset.h>
24 #include <gtsam/inference/Symbol.h>
25 
26 #include <vector>
27 
28 using namespace std;
29 using namespace gtsam;
32 
33 // We will be using a projection factor that ties a SFM_Camera to a 3D point.
34 // An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
35 // and has a total of 9 free parameters
37 
38 /* ************************************************************************* */
39 int main (int argc, char* argv[]) {
40 
41  // Find default file, but if an argument is given, try loading a file
42  string filename = findExampleDataFile("dubrovnik-3-7-pre");
43  if (argc>1) filename = string(argv[1]);
44 
45  // Load the SfM data from file
46  SfmData mydata = SfmData::FromBalFile(filename);
47  cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl;
48 
49  // Create a factor graph
51 
52  // We share *one* noiseModel between all projection factors
53  auto noise =
54  noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
55 
56  // Add measurements to the factor graph
57  size_t j = 0;
58  for(const SfmTrack& track: mydata.tracks) {
59  for (const auto& [i, uv] : track.measurements) {
60  graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
61  }
62  j += 1;
63  }
64 
65  // Add a prior on pose x1. This indirectly specifies where the origin is.
66  // and a prior on the position of the first landmark to fix the scale
67  graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
68  graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
69 
70  // Create initial estimate
72  size_t i = 0; j = 0;
73  for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
74  for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p);
75 
76  /* Optimize the graph and print results */
77  Values result;
78  try {
80  params.setVerbosity("ERROR");
81  LevenbergMarquardtOptimizer lm(graph, initial, params);
82  result = lm.optimize();
83  } catch (exception& e) {
84  cout << e.what();
85  }
86  cout << "final error: " << graph.error(result) << endl;
87 
88  return 0;
89 }
90 /* ************************************************************************* */
91 
virtual const Values & optimize()
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
Factor Graph consisting of non-linear factors.
Values initial
Definition: BFloat16.h:88
NonlinearFactorGraph graph
GeneralSFMFactor< SfmCamera, Point3 > MyFactor
static const SmartProjectionParams params
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Values result
void setVerbosity(const std::string &src)
Data structure for dealing with Structure from Motion data.
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition: SfmTrack.h:44
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
size_t numberCameras() const
The number of cameras.
Definition: SfmData.h:77
Array< double, 1, 3 > e(1./3., 0.5, 2.)
double error(const Values &values) const
int main(int argc, char *argv[])
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
traits
Definition: chartTesting.h:28
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
Point3 p
3D position of the point
Definition: SfmTrack.h:126
void insert(Key j, const Value &val)
Definition: Values.cpp:155
size_t numberTracks() const
The number of reconstructed 3D points.
Definition: SfmData.h:74
static const CalibratedCamera camera(kDefaultPose)
a general SFM factor with an unknown calibration
utility functions for loading datasets
std::ptrdiff_t j


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