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.cpp
14 * @brief time SFM with BAL file, conventional GeneralSFMFactor
15 * @author Frank Dellaert
16 * @date June 6, 2015
17 */
18
19#include "timeSFMBAL.h"
20
21#include <gtsam/slam/GeneralSFMFactor.h>
22#include <gtsam/geometry/Cal3Bundler.h>
23#include <gtsam/geometry/PinholeCamera.h>
24#include <gtsam/geometry/Point3.h>
25
26
27using namespace std;
28using namespace gtsam;
29
30typedef PinholeCamera<Cal3Bundler> Camera;
31typedef GeneralSFMFactor<Camera, Point3> SfmFactor;
32
33int main(int argc, char* argv[]) {
34 // parse options and read BAL file
35 SfmData db = preamble(argc, argv);
36
37 // Build graph using conventional GeneralSFMFactor
38 NonlinearFactorGraph graph;
39 for (size_t j = 0; j < db.numberTracks(); j++) {
40 for (const SfmMeasurement& m: db.tracks[j].measurements) {
41 size_t i = m.first;
42 Point2 z = m.second;
43 graph.emplace_shared<SfmFactor>(args&: z, args&: gNoiseModel, args: C(j: i), args: P(j));
44 }
45 }
46
47 Values initial;
48 size_t i = 0, j = 0;
49 for (const SfmCamera& camera: db.cameras)
50 initial.insert(j: C(j: i++), val: camera);
51 for (const SfmTrack& track: db.tracks)
52 initial.insert(j: P(j: j++), val: track.p);
53
54 return optimize(db, graph, initial);
55}
56