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 timeSFMBALcamTnav.cpp
14 * @brief time SFM with BAL file, expressions with camTnav pose
15 * @author Frank Dellaert
16 * @date July 5, 2015
17 */
18
19#include "timeSFMBAL.h"
20
21#include <gtsam/slam/expressions.h>
22#include <gtsam/nonlinear/ExpressionFactor.h>
23#include <gtsam/geometry/Cal3Bundler.h>
24#include <gtsam/geometry/Point3.h>
25
26
27using namespace std;
28using namespace gtsam;
29
30int main(int argc, char* argv[]) {
31 // parse options and read BAL file
32 SfmData db = preamble(argc, argv);
33
34 // Build graph using conventional GeneralSFMFactor
35 NonlinearFactorGraph graph;
36 for (size_t j = 0; j < db.numberTracks(); j++) {
37 for (const SfmMeasurement& m: db.tracks[j].measurements) {
38 size_t i = m.first;
39 Point2 z = m.second;
40 Pose3_ camTnav_(C(j: i));
41 Cal3Bundler_ calibration_(K(j: i));
42 Point3_ nav_point_(P(j));
43 graph.addExpressionFactor(
44 R: gNoiseModel, z,
45 h: uncalibrate(K: calibration_, // now using transformFrom !!!:
46 xy_hat: project(p_cam: transformFrom(x: camTnav_, p: nav_point_))));
47 }
48 }
49
50 Values initial;
51 size_t i = 0, j = 0;
52 for (const SfmCamera& camera: db.cameras) {
53 initial.insert(j: C(j: i), val: camera.pose().inverse()); // inverse !!!
54 initial.insert(j: K(j: i), val: camera.calibration());
55 i += 1;
56 }
57 for (const SfmTrack& track: db.tracks)
58 initial.insert(j: P(j: j++), val: track.p);
59
60 bool separateCalibration = true;
61 return optimize(db, graph, initial, separateCalibration);
62}
63