SFMExampleExpressions_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 
24 // The two new headers that allow using our Automatic Differentiation Expression framework
25 #include <gtsam/slam/expressions.h>
27 
28 // Header order is close to far
29 #include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
30 #include <gtsam/slam/dataset.h>
32 #include <gtsam/inference/Symbol.h>
33 
34 #include <vector>
35 
36 using namespace std;
37 using namespace gtsam;
38 using namespace noiseModel;
41 
42 // An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration
43 // and has a total of 9 free parameters
44 
45 int main(int argc, char* argv[]) {
46  // Find default file, but if an argument is given, try loading a file
47  string filename = findExampleDataFile("dubrovnik-3-7-pre");
48  if (argc > 1) filename = string(argv[1]);
49 
50  // Load the SfM data from file
51  SfmData mydata = SfmData::FromBalFile(filename);
52  cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl;
53 
54  // Create a factor graph
56 
57  // Here we don't use a PriorFactor but directly the ExpressionFactor class
58  // First, we create an expression to the pose from the first camera
59  Expression<SfmCamera> camera0_(C(0));
60  // Then, to get its pose:
61  Pose3_ pose0_(&SfmCamera::getPose, camera0_);
62  // Finally, we say it should be equal to first guess
63  graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
64  noiseModel::Isotropic::Sigma(6, 0.1));
65 
66  // similarly, we create a prior on the first point
67  Point3_ point0_(P(0));
68  graph.addExpressionFactor(point0_, mydata.tracks[0].p,
69  noiseModel::Isotropic::Sigma(3, 0.1));
70 
71  // We share *one* noiseModel between all projection factors
72  auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
73 
74  // Simulated measurements from each camera pose, adding them to the factor
75  // graph
76  size_t j = 0;
77  for (const SfmTrack& track : mydata.tracks) {
78  // Leaf expression for j^th point
79  Point3_ point_('p', j);
80  for (const auto& [i, uv] : track.measurements) {
81  // Leaf expression for i^th camera
82  Expression<SfmCamera> camera_(C(i));
83  // Below an expression for the prediction of the measurement:
84  Point2_ predict_ = project2<SfmCamera>(camera_, point_);
85  // Again, here we use an ExpressionFactor
86  graph.addExpressionFactor(predict_, uv, noise);
87  }
88  j += 1;
89  }
90 
91  // Create initial estimate
93  size_t i = 0;
94  j = 0;
95  for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
96  for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
97 
98  /* Optimize the graph and print results */
99  Values result;
100  try {
102  params.setVerbosity("ERROR");
103  LevenbergMarquardtOptimizer lm(graph, initial, params);
104  result = lm.optimize();
105  } catch (exception& e) {
106  cout << e.what();
107  }
108  cout << "final error: " << graph.error(result) << endl;
109 
110  return 0;
111 }
Pose3_ getPose(const Expression< PinholeCamera< CALIBRATION > > &cam)
virtual const Values & optimize()
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
Values initial
Definition: BFloat16.h:88
NonlinearFactorGraph graph
static const SmartProjectionParams params
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
Values result
Factor graph that supports adding ExpressionFactors directly.
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
void addExpressionFactor(const Expression< T > &h, const T &z, const SharedNoiseModel &R)
size_t numberTracks() const
The number of reconstructed 3D points.
Definition: SfmData.h:74
static const CalibratedCamera camera(kDefaultPose)
utility functions for loading datasets
std::ptrdiff_t j


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