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 SFMExample_bal.cpp
14 * @brief Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
15 * @author Frank Dellaert
16 */
17
18// For an explanation of headers, see SFMExample.cpp
19#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
20#include <gtsam/slam/GeneralSFMFactor.h>
21#include <gtsam/slam/dataset.h>
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
23#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
24#include <gtsam/inference/Symbol.h>
25
26#include <vector>
27
28using namespace std;
29using namespace gtsam;
30using symbol_shorthand::C;
31using symbol_shorthand::P;
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
36typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
37
38/* ************************************************************************* */
39int main (int argc, char* argv[]) {
40
41 // Find default file, but if an argument is given, try loading a file
42 string filename = findExampleDataFile(name: "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
50 NonlinearFactorGraph graph;
51
52 // We share *one* noiseModel between all projection factors
53 auto noise =
54 noiseModel::Isotropic::Sigma(dim: 2, sigma: 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>(args: uv, args&: noise, args: C(j: i), args: 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(key: C(j: 0), prior: mydata.cameras[0], model: noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
68 graph.addPrior(key: P(j: 0), prior: mydata.tracks[0].p, model: noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1));
69
70 // Create initial estimate
71 Values initial;
72 size_t i = 0; j = 0;
73 for(const SfmCamera& camera: mydata.cameras) initial.insert(j: C(j: i++), val: camera);
74 for(const SfmTrack& track: mydata.tracks) initial.insert(j: P(j: j++), val: track.p);
75
76 /* Optimize the graph and print results */
77 Values result;
78 try {
79 LevenbergMarquardtParams params;
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(values: result) << endl;
87
88 return 0;
89}
90/* ************************************************************************* */
91
92