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");
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 }
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::SfmTrack2d::measurements
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition: SfmTrack.h:44
gtsam::ExpressionFactorGraph
Definition: ExpressionFactorGraph.h:29
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
gtsam::SfmTrack
Definition: SfmTrack.h:125
gtsam::Expression
Definition: Expression.h:47
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
gtsam::SfmData::tracks
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
dataset.h
utility functions for loading datasets
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
relicense.filename
filename
Definition: relicense.py:57
gtsam::PinholeCamera< Cal3Bundler >
ExpressionFactorGraph.h
Factor graph that supports adding ExpressionFactors directly.
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Symbol.h
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.
gtsam::getPose
Pose3_ getPose(const Expression< PinholeCamera< CALIBRATION > > &cam)
Definition: slam/expressions.h:177
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
main
int main(int argc, char *argv[])
Definition: SFMExampleExpressions_bal.cpp:45
gtsam::SfmData::cameras
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
gtsam::Values
Definition: Values.h:65
expressions.h
Common expressions for solving geometry/slam/sfm problems.
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
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::SfmTrack::p
Point3 p
3D position of the point
Definition: SfmTrack.h:126
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
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
gtsam::NonlinearFactorGraph::addExpressionFactor
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Definition: NonlinearFactorGraph.h:187


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:11