gtsam 4.2.0
gtsam
SfmData.h
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#pragma once
20
25#include <gtsam/sfm/SfmTrack.h>
26
27#include <string>
28#include <vector>
29
30namespace gtsam {
31
34
39struct GTSAM_EXPORT SfmData {
40 std::vector<SfmCamera> cameras;
41
42 std::vector<SfmTrack> tracks;
43
46
53 static SfmData FromBundlerFile(const std::string& filename);
54
61 static SfmData FromBalFile(const std::string& filename);
62
66
68 void addTrack(const SfmTrack& t) { tracks.push_back(t); }
69
71 void addCamera(const SfmCamera& cam) { cameras.push_back(cam); }
72
74 size_t numberTracks() const { return tracks.size(); }
75
77 size_t numberCameras() const { return cameras.size(); }
78
80 const SfmTrack& track(size_t idx) const { return tracks[idx]; }
81
83 const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
84
86 const std::vector<SfmCamera>& cameraList() const { return cameras; }
87 const std::vector<SfmTrack>& trackList() const { return tracks; }
88
95 NonlinearFactorGraph generalSfmFactors(
97 1.0)) const;
98
109 NonlinearFactorGraph sfmFactorGraph(
110 const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0),
111 boost::optional<size_t> fixedCamera = 0,
112 boost::optional<size_t> fixedPoint = 0) const;
113
117
119 void print(const std::string& s = "") const;
120
122 bool equals(const SfmData& sfmData, double tol = 1e-9) const;
123
125#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
128 void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); }
129 void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) {
130 cameras.push_back(cam);
131 }
132 size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); }
133 size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); }
135#endif
138
140 friend class boost::serialization::access;
141 template <class Archive>
142 void serialize(Archive& ar, const unsigned int /*version*/) {
143 ar& BOOST_SERIALIZATION_NVP(cameras);
144 ar& BOOST_SERIALIZATION_NVP(tracks);
145 }
146
148};
149
151template <>
152struct traits<SfmData> : public Testable<SfmData> {};
153
154#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
155GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename,
156 SfmData& data);
157GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename,
158 SfmData& data);
159#endif
160
167GTSAM_EXPORT SfmData readBal(const std::string& filename);
168
176GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data);
177
190GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
191 const SfmData& data, const Values& values);
192
201GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
202
211GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
212
218GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
219
228GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
229
238GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
239
240} // namespace gtsam
Base class for all pinhole cameras.
Calibration used by Bundler.
Factor Graph consisting of non-linear factors.
A non-templated config holding any types of Manifold-group elements.
A simple data structure for a track in Structure from Motion.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
bool writeBAL(const std::string &filename, const SfmData &data)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
Definition: SfmData.cpp:249
bool writeBALfromValues(const std::string &filename, const SfmData &data, const Values &values)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a val...
Definition: SfmData.cpp:349
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
SfmData readBal(const std::string &filename)
This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData ...
Definition: SfmData.cpp:344
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
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
Values initialCamerasAndPointsEstimate(const SfmData &db)
This function creates initial values for cameras and points from db.
Definition: SfmData.cpp:449
PinholeCamera< Cal3Bundler > SfmCamera
Define the structure for the camera poses.
Definition: SfmData.h:33
Values initialCamerasEstimate(const SfmData &db)
This function creates initial values for cameras from db.
Definition: SfmData.cpp:441
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz)
This function converts an openGL camera pose to an GTSAM camera pose.
Definition: SfmData.cpp:79
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A pinhole camera class that has a Pose3 and a Calibration.
Definition: PinholeCamera.h:33
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition: NoiseModel.cpp:597
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
const std::vector< SfmCamera > & cameraList() const
Getters.
Definition: SfmData.h:86
void addCamera(const SfmCamera &cam)
Add a camera to SfmData.
Definition: SfmData.h:71
size_t numberCameras() const
The number of cameras.
Definition: SfmData.h:77
size_t numberTracks() const
The number of reconstructed 3D points.
Definition: SfmData.h:74
const SfmTrack & track(size_t idx) const
The track formed by series of landmark measurements.
Definition: SfmData.h:80
void addTrack(const SfmTrack &t)
Add a track to SfmData.
Definition: SfmData.h:68
const SfmCamera & camera(size_t idx) const
The camera pose at frame index idx
Definition: SfmData.h:83
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
Definition: SfmTrack.h:126