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
12/**
13 * @file SFMExampleExpressions_bal.cpp
14 * @brief A structure-from-motion example done with Expressions
15 * @author Frank Dellaert
16 * @date January 2015
17 */
18
19/**
20 * This is the Expression version of SFMExample
21 * See detailed description of headers there, this focuses on explaining the AD part
22 */
23
24// The two new headers that allow using our Automatic Differentiation Expression framework
25#include <gtsam/slam/expressions.h>
26#include <gtsam/nonlinear/ExpressionFactorGraph.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>
31#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
32#include <gtsam/inference/Symbol.h>
33
34#include <vector>
35
36using namespace std;
37using namespace gtsam;
38using namespace noiseModel;
39using symbol_shorthand::C;
40using symbol_shorthand::P;
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
45int main(int argc, char* argv[]) {
46 // Find default file, but if an argument is given, try loading a file
47 string filename = findExampleDataFile(name: "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
55 ExpressionFactorGraph 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(j: 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(h: pose0_, z: mydata.cameras[0].pose(),
64 R: noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.1));
65
66 // similarly, we create a prior on the first point
67 Point3_ point0_(P(j: 0));
68 graph.addExpressionFactor(h: point0_, z: mydata.tracks[0].p,
69 R: noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1));
70
71 // We share *one* noiseModel between all projection factors
72 auto noise = noiseModel::Isotropic::Sigma(dim: 2, sigma: 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(j: i));
83 // Below an expression for the prediction of the measurement:
84 Point2_ predict_ = project2<SfmCamera>(camera_, p_: point_);
85 // Again, here we use an ExpressionFactor
86 graph.addExpressionFactor(h: predict_, z: uv, R: noise);
87 }
88 j += 1;
89 }
90
91 // Create initial estimate
92 Values initial;
93 size_t i = 0;
94 j = 0;
95 for (const SfmCamera& camera : mydata.cameras) initial.insert(j: C(j: i++), val: camera);
96 for (const SfmTrack& track : mydata.tracks) initial.insert(j: P(j: j++), val: track.p);
97
98 /* Optimize the graph and print results */
99 Values result;
100 try {
101 LevenbergMarquardtParams params;
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(values: result) << endl;
109
110 return 0;
111}
112