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 timeSFMBAL.h
14 * @brief Common code for timeSFMBAL scripts
15 * @author Frank Dellaert
16 * @date July 5, 2015
17 */
18
19#pragma once
20
21#include <gtsam/sfm/SfmData.h>
22#include <gtsam/slam/dataset.h>
23#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
24#include <gtsam/nonlinear/NonlinearFactorGraph.h>
25#include <gtsam/nonlinear/Values.h>
26#include <gtsam/linear/NoiseModel.h>
27#include <gtsam/inference/Ordering.h>
28#include <gtsam/inference/Symbol.h>
29#include <gtsam/base/timing.h>
30
31#include <string>
32#include <vector>
33
34using namespace std;
35using namespace gtsam;
36using symbol_shorthand::C;
37using symbol_shorthand::K;
38using symbol_shorthand::P;
39
40static bool gUseSchur = true;
41static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(dim: 2);
42
43// parse options and read BAL file
44SfmData preamble(int argc, char* argv[]) {
45 // primitive argument parsing:
46 if (argc > 2) {
47 if (strcmp(s1: argv[1], s2: "--colamd"))
48 gUseSchur = false;
49 else
50 throw runtime_error("Usage: timeSFMBALxxx [--colamd] [BALfile]");
51 }
52
53 // Load BAL file
54 SfmData db;
55 string filename;
56 if (argc > 1)
57 filename = argv[argc - 1];
58 else
59 filename = findExampleDataFile(name: "dubrovnik-16-22106-pre");
60 return SfmData::FromBalFile(filename);
61}
62
63// Create ordering and optimize
64int optimize(const SfmData& db, const NonlinearFactorGraph& graph,
65 const Values& initial, bool separateCalibration = false) {
66 using symbol_shorthand::P;
67
68 // Set parameters to be similar to ceres
69 LevenbergMarquardtParams params;
70 LevenbergMarquardtParams::SetCeresDefaults(&params);
71// params.setLinearSolverType("SEQUENTIAL_CHOLESKY");
72// params.setVerbosityLM("SUMMARY");
73
74 if (gUseSchur) {
75 // Create Schur-complement ordering
76 Ordering ordering;
77 for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(x: P(j));
78 for (size_t i = 0; i < db.numberCameras(); i++) {
79 ordering.push_back(x: C(j: i));
80 if (separateCalibration) ordering.push_back(x: K(j: i));
81 }
82 params.setOrdering(ordering);
83 }
84
85 // Optimize
86 {
87 gttic_(optimize);
88 LevenbergMarquardtOptimizer lm(graph, initial, params);
89 Values result = lm.optimize();
90 }
91
92 tictoc_finishedIteration_();
93 tictoc_print_();
94
95 return 0;
96}
97