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 timeSFMBALautodiff.cpp
14 * @brief time SFM with BAL file, Ceres autodiff version
15 * @author Frank Dellaert
16 * @date July 5, 2015
17 */
18
19#include "timeSFMBAL.h"
20#include <gtsam/geometry/Point3.h>
21#include <gtsam/nonlinear/ExpressionFactor.h>
22#include <gtsam/nonlinear/AdaptAutoDiff.h>
23#include <gtsam/3rdparty/ceres/example.h>
24
25#include <stddef.h>
26#include <stdexcept>
27#include <string>
28
29using namespace std;
30using namespace gtsam;
31
32// See http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.3-manual.html
33// as to why so much gymnastics is needed to massage the initial estimates and
34// measurements: basically, Snavely does not use computer vision conventions
35// but OpenGL conventions :-(
36
37typedef PinholeCamera<Cal3Bundler> Camera;
38
39int main(int argc, char* argv[]) {
40 // parse options and read BAL file
41 SfmData db = preamble(argc, argv);
42
43 AdaptAutoDiff<SnavelyProjection, 2, 9, 3> snavely;
44
45 // Build graph
46 NonlinearFactorGraph graph;
47 for (size_t j = 0; j < db.numberTracks(); j++) {
48 for (const SfmMeasurement& m: db.tracks[j].measurements) {
49 size_t i = m.first;
50 Point2 z = m.second;
51 Expression<Vector9> camera_(C(j: i));
52 Expression<Vector3> point_(P(j));
53 // Expects measurements in OpenGL format, with y increasing upwards
54 graph.addExpressionFactor(R: gNoiseModel, z: Vector2(z.x(), -z.y()),
55 h: Expression<Vector2>(snavely, camera_, point_));
56 }
57 }
58
59 Values initial;
60 size_t i = 0, j = 0;
61 for (const SfmCamera& camera: db.cameras) {
62 // SfmData::FromBalFile converts to GTSAM format, so we need to convert back !
63 Pose3 openGLpose = gtsam2openGL(PoseGTSAM: camera.pose());
64 Vector9 v9;
65 v9 << Pose3::Logmap(pose: openGLpose), camera.calibration();
66 initial.insert(j: C(j: i++), val: v9);
67 }
68 for (const SfmTrack& track: db.tracks) {
69 Vector3 v3 = track.p;
70 initial.insert(j: P(j: j++), val: v3);
71 }
72
73 return optimize(db, graph, initial);
74}
75