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_COLAMD_METIS.cpp
14 * @brief This file is to compare the ordering performance for COLAMD vs METIS.
15 * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
16 * @author Frank Dellaert, Zhaoyang Lv
17 */
18
19// For an explanation of headers, see SFMExample.cpp
20#include <gtsam/slam/GeneralSFMFactor.h>
21#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
22#include <gtsam/slam/dataset.h>
23#include <gtsam/nonlinear/NonlinearFactorGraph.h>
24#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
25#include <gtsam/inference/Symbol.h>
26#include <gtsam/inference/Ordering.h>
27#include <gtsam/base/timing.h>
28
29#include <vector>
30
31using namespace std;
32using namespace gtsam;
33using symbol_shorthand::C;
34using symbol_shorthand::P;
35
36// We will be using a projection factor that ties a SFM_Camera to a 3D point.
37// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler
38// calibration and has a total of 9 free parameters
39typedef GeneralSFMFactor<SfmCamera, Point3> MyFactor;
40
41
42int main(int argc, char* argv[]) {
43 // Find default file, but if an argument is given, try loading a file
44 string filename = findExampleDataFile(name: "dubrovnik-3-7-pre");
45 if (argc > 1) filename = string(argv[1]);
46
47 // Load the SfM data from file
48 SfmData mydata = SfmData::FromBalFile(filename);
49 cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl;
50
51 // Create a factor graph
52 NonlinearFactorGraph graph;
53
54 // We share *one* noiseModel between all projection factors
55 auto noise = noiseModel::Isotropic::Sigma(dim: 2, sigma: 1.0); // one pixel in u and v
56
57 // Add measurements to the factor graph
58 size_t j = 0;
59 for (const SfmTrack& track : mydata.tracks) {
60 for (const auto& [i, uv] : track.measurements) {
61 graph.emplace_shared<MyFactor>(
62 args: uv, args&: noise, args: C(j: i), args: P(j)); // note use of shorthand symbols C and P
63 }
64 j += 1;
65 }
66
67 // Add a prior on pose x1. This indirectly specifies where the origin is.
68 // and a prior on the position of the first landmark to fix the scale
69 graph.addPrior(key: C(j: 0), prior: mydata.cameras[0], model: noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.1));
70 graph.addPrior(key: P(j: 0), prior: mydata.tracks[0].p,
71 model: noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1));
72
73 // Create initial estimate
74 Values initial;
75 size_t i = 0;
76 j = 0;
77 for (const SfmCamera& camera : mydata.cameras) initial.insert(j: C(j: i++), val: camera);
78 for (const SfmTrack& track : mydata.tracks) initial.insert(j: P(j: j++), val: track.p);
79
80 /** --------------- COMPARISON -----------------------**/
81 /** ----------------------------------------------------**/
82
83 LevenbergMarquardtParams params_using_COLAMD, params_using_METIS;
84 try {
85 params_using_METIS.setVerbosity("ERROR");
86 gttic_(METIS_ORDERING);
87 params_using_METIS.ordering = Ordering::Create(orderingType: Ordering::METIS, graph);
88 gttoc_(METIS_ORDERING);
89
90 params_using_COLAMD.setVerbosity("ERROR");
91 gttic_(COLAMD_ORDERING);
92 params_using_COLAMD.ordering = Ordering::Create(orderingType: Ordering::COLAMD, graph);
93 gttoc_(COLAMD_ORDERING);
94 } catch (exception& e) {
95 cout << e.what();
96 }
97
98 // expect they have different ordering results
99 if (params_using_COLAMD.ordering == params_using_METIS.ordering) {
100 cout << "COLAMD and METIS produce the same ordering. "
101 << "Problem here!!!" << endl;
102 }
103
104 /* Optimize the graph with METIS and COLAMD and time the results */
105
106 Values result_METIS, result_COLAMD;
107 try {
108 gttic_(OPTIMIZE_WITH_METIS);
109 LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS);
110 result_METIS = lm_METIS.optimize();
111 gttoc_(OPTIMIZE_WITH_METIS);
112
113 gttic_(OPTIMIZE_WITH_COLAMD);
114 LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD);
115 result_COLAMD = lm_COLAMD.optimize();
116 gttoc_(OPTIMIZE_WITH_COLAMD);
117 } catch (exception& e) {
118 cout << e.what();
119 }
120
121 { // printing the result
122
123 cout << "COLAMD final error: " << graph.error(values: result_COLAMD) << endl;
124 cout << "METIS final error: " << graph.error(values: result_METIS) << endl;
125
126 cout << endl << endl;
127
128 cout << "Time comparison by solving " << filename << " results:" << endl;
129
130 cout << mydata.numberTracks() << " point tracks and " << mydata.numberCameras()
131 << " cameras" << endl;
132
133 tictoc_print_();
134 }
135
136 return 0;
137}
138
139