timeSFMBALautodiff.cpp
Go to the documentation of this file.
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 
19 #include "timeSFMBAL.h"
20 #include <gtsam/geometry/Point3.h>
24 
25 #include <stddef.h>
26 #include <stdexcept>
27 #include <string>
28 
29 using namespace std;
30 using 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 
38 
39 int main(int argc, char* argv[]) {
40  // parse options and read BAL file
41  SfmData db = preamble(argc, argv);
42 
44 
45  // Build 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(i));
52  Expression<Vector3> point_(P(j));
53  // Expects measurements in OpenGL format, with y increasing upwards
54  graph.addExpressionFactor(gNoiseModel, Vector2(z.x(), -z.y()),
55  Expression<Vector2>(snavely, camera_, point_));
56  }
57  }
58 
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(camera.pose());
64  Vector9 v9;
65  v9 << Pose3::Logmap(openGLpose), camera.calibration();
66  initial.insert(C(i++), v9);
67  }
68  for (const SfmTrack& track: db.tracks) {
69  Vector3 v3 = track.p;
70  initial.insert(P(j++), v3);
71  }
72 
73  return optimize(db, graph, initial);
74 }
Matrix3f m
Adaptor for Ceres style auto-differentiated functions.
static SharedNoiseModel gNoiseModel
Definition: timeSFMBAL.h:41
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector2 Point2
Definition: Point2.h:32
Values initial
Definition: BFloat16.h:88
int main(int argc, char *argv[])
const Calibration & calibration() const override
return calibration
NonlinearFactorGraph graph
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
const Pose3 & pose() const
return pose
Common code for timeSFMBAL scripts.
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
Definition: SfmTrack.h:32
Vector v3
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
traits
Definition: chartTesting.h:28
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
Eigen::Vector2d Vector2
Definition: Vector.h:42
SfmData preamble(int argc, char *argv[])
Definition: timeSFMBAL.h:44
Point3 p
3D position of the point
Definition: SfmTrack.h:126
3D Point
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
void insert(Key j, const Value &val)
Definition: Values.cpp:155
size_t numberTracks() const
The number of reconstructed 3D points.
Definition: SfmData.h:74
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz)
This function converts a GTSAM camera pose to an openGL camera pose.
Definition: SfmData.cpp:88
static const CalibratedCamera camera(kDefaultPose)
PinholeCamera< Cal3Bundler > Camera
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:09