gtsam  4.1.0
gtsam
dataset.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 
21 #pragma once
22 
27 #include <gtsam/geometry/Pose2.h>
28 #include <gtsam/geometry/Pose3.h>
30 #include <gtsam/nonlinear/Values.h>
32 #include <gtsam/base/types.h>
33 
34 #include <boost/smart_ptr/shared_ptr.hpp>
35 #include <string>
36 #include <utility> // for pair
37 #include <vector>
38 #include <iosfwd>
39 #include <map>
40 
41 namespace gtsam {
42 
54 GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
55 
60 GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
61 
69 };
70 
73  KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
74 };
75 
82 template <typename T>
83 GTSAM_EXPORT std::map<size_t, T> parseVariables(const std::string &filename,
84  size_t maxIndex = 0);
85 
92 template <typename T>
93 GTSAM_EXPORT std::vector<BinaryMeasurement<T>>
94 parseMeasurements(const std::string &filename,
95  const noiseModel::Diagonal::shared_ptr &model = nullptr,
96  size_t maxIndex = 0);
97 
102 template <typename T>
103 GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr>
104 parseFactors(const std::string &filename,
105  const noiseModel::Diagonal::shared_ptr &model = nullptr,
106  size_t maxIndex = 0);
107 
109 typedef std::pair<size_t, Pose2> IndexedPose;
110 typedef std::pair<size_t, Point2> IndexedLandmark;
111 typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;
112 
118 GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
119  const std::string& tag);
120 
126 GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
127  const std::string& tag);
128 
134 GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
135  const std::string& tag);
136 
141  std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;
142 
150 GTSAM_EXPORT GraphAndValues load2D(
151  std::pair<std::string, SharedNoiseModel> dataset, size_t maxIndex = 0,
152  bool addNoise = false,
153  bool smart = true, //
154  NoiseFormat noiseFormat = NoiseFormatAUTO,
155  KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
156 
168 GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
169  SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise =
170  false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
171  KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
172 
174 GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
175  const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
176 
178 GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
179  const Values& config, const noiseModel::Diagonal::shared_ptr model,
180  const std::string& filename);
181 
190 GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false,
191  KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
192 
205 GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
206  const Values& estimate, const std::string& filename);
207 
209 GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
210 
212 typedef std::pair<size_t, Point2> SfmMeasurement;
213 
215 typedef std::pair<size_t, size_t> SiftIndex;
216 
218 struct SfmTrack {
219  SfmTrack(): p(0,0,0) {}
220  SfmTrack(const gtsam::Point3& pt) : p(pt) {}
222  float r, g, b;
223  std::vector<SfmMeasurement> measurements;
224  std::vector<SiftIndex> siftIndices;
225 
227  size_t number_measurements() const {
228  return measurements.size();
229  }
231  SfmMeasurement measurement(size_t idx) const {
232  return measurements[idx];
233  }
235  SiftIndex siftIndex(size_t idx) const {
236  return siftIndices[idx];
237  }
239  const Point3& point3() const {
240  return p;
241  }
243  void add_measurement(size_t idx, const gtsam::Point2& m) {
244  measurements.emplace_back(idx, m);
245  }
246 };
247 
248 
251 
253 struct SfmData {
254  std::vector<SfmCamera> cameras;
255  std::vector<SfmTrack> tracks;
256  size_t number_cameras() const {
257  return cameras.size();
258  }
260  size_t number_tracks() const {
261  return tracks.size();
262  }
264  SfmCamera camera(size_t idx) const {
265  return cameras[idx];
266  }
268  SfmTrack track(size_t idx) const {
269  return tracks[idx];
270  }
272  void add_track(const SfmTrack& t) {
273  tracks.push_back(t);
274  }
276  void add_camera(const SfmCamera& cam){
277  cameras.push_back(cam);
278  }
279 };
280 
288 GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data);
289 
297 GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data);
298 
305 GTSAM_EXPORT SfmData readBal(const std::string& filename);
306 
314 GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData &data);
315 
327 GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
328  const SfmData &data, Values& values);
329 
338 GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
339 
348 GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
349 
355 GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
356 
362 GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
363 
369 GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
370 
371 // Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
372 using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
373 GTSAM_EXPORT BetweenFactorPose2s
374 parse2DFactors(const std::string &filename,
375  const noiseModel::Diagonal::shared_ptr &model = nullptr,
376  size_t maxIndex = 0);
377 
378 using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
379 GTSAM_EXPORT BetweenFactorPose3s
380 parse3DFactors(const std::string &filename,
381  const noiseModel::Diagonal::shared_ptr &model = nullptr,
382  size_t maxIndex = 0);
383 
384 using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
385 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
386 inline boost::optional<IndexedPose> parseVertex(std::istream &is,
387  const std::string &tag) {
388  return parseVertexPose(is, tag);
389 }
390 
391 GTSAM_EXPORT std::map<size_t, Pose3> parse3DPoses(const std::string &filename,
392  size_t maxIndex = 0);
393 
394 GTSAM_EXPORT std::map<size_t, Point3>
395 parse3DLandmarks(const std::string &filename, size_t maxIndex = 0);
396 
397 #endif
398 } // namespace gtsam
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
gtsam::NoiseFormatGRAPH
@ NoiseFormatGRAPH
default: toro-style order, but covariance matrix !
Definition: dataset.h:66
gtsam::SfmData
Define the structure for SfM data.
Definition: dataset.h:253
gtsam::writeBALfromValues
bool writeBALfromValues(const string &filename, const SfmData &data, Values &values)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a val...
Definition: dataset.cpp:1216
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::load2D
GraphAndValues load2D(const string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Load TORO/G2O style graph files.
Definition: dataset.cpp:500
gtsam::SfmTrack::point3
const Point3 & point3() const
Get 3D point.
Definition: dataset.h:239
gtsam::readBAL
bool readBAL(const string &filename, SfmData &data)
This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a SfmData...
Definition: dataset.cpp:1073
gtsam::SfmData::add_camera
void add_camera(const SfmCamera &cam)
Add a camera to SfmData.
Definition: dataset.h:276
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::gtsam2openGL
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz)
This function converts a GTSAM camera pose to an openGL camera pose.
Definition: dataset.cpp:972
gtsam::readBal
SfmData readBal(const string &filename)
This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData ...
Definition: dataset.cpp:1133
gtsam::SfmData::cameras
std::vector< SfmCamera > cameras
Set of cameras.
Definition: dataset.h:254
gtsam::writeG2o
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure.
Definition: dataset.cpp:630
gtsam::parseMeasurements
std::vector< BinaryMeasurement< Pose2 > > parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Parse binary measurements in a line-based text format (like g2o) into a vector.
Definition: dataset.cpp:390
BetweenFactor.h
gtsam::parseVariables
GTSAM_EXPORT std::map< size_t, T > parseVariables(const std::string &filename, size_t maxIndex=0)
Parse variables in a line-based text format (like g2o) into a map.
gtsam::NoiseFormatTORO
@ NoiseFormatTORO
Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
Definition: dataset.h:65
types.h
Typedefs for easier changing of types.
Pose3.h
3D Pose
gtsam::NoiseFormatG2O
@ NoiseFormatG2O
Information matrix I11, I12, I13, I22, I23, I33.
Definition: dataset.h:64
gtsam::Pose2
Definition: Pose2.h:36
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam::SfmTrack::p
Point3 p
3D position of the point
Definition: dataset.h:221
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::parseEdge
boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag)
Parse TORO/G2O edge "id1 id2 x y yaw".
Definition: dataset.cpp:294
gtsam::initialCamerasEstimate
Values initialCamerasEstimate(const SfmData &db)
This function creates initial values for cameras from db.
Definition: dataset.cpp:1276
gtsam::SfmCamera
PinholeCamera< Cal3Bundler > SfmCamera
Define the structure for the camera poses.
Definition: dataset.h:250
gtsam::SfmTrack::add_measurement
void add_measurement(size_t idx, const gtsam::Point2 &m)
Add measurement (camera_idx, Point2) to track.
Definition: dataset.h:243
gtsam::openGL2gtsam
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz)
This function converts an openGL camera pose to an GTSAM camera pose.
Definition: dataset.cpp:963
gtsam::SiftIndex
std::pair< size_t, size_t > SiftIndex
Sift index for SfmTrack.
Definition: dataset.h:215
gtsam::initialCamerasAndPointsEstimate
Values initialCamerasAndPointsEstimate(const SfmData &db)
This function creates initial values for cameras and points from db.
Definition: dataset.cpp:1284
Pose2.h
2D Pose
gtsam::SfmTrack::number_measurements
size_t number_measurements() const
Total number of measurements in this track.
Definition: dataset.h:227
gtsam::load3D
GraphAndValues load3D(const string &filename)
Load TORO 3D Graph.
Definition: dataset.cpp:924
gtsam::readBundler
bool readBundler(const string &filename, SfmData &data)
This function parses a bundler output file and stores the data into a SfmData structure.
Definition: dataset.cpp:986
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
gtsam::SfmData::track
SfmTrack track(size_t idx) const
The track formed by series of landmark measurements.
Definition: dataset.h:268
gtsam::SfmTrack::measurement
SfmMeasurement measurement(size_t idx) const
Get the measurement (camera index, Point2) at pose index idx
Definition: dataset.h:231
gtsam::SfmTrack
Define the structure for the 3D points.
Definition: dataset.h:218
gtsam::SfmMeasurement
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
Definition: dataset.h:212
gtsam::SfmTrack::siftIndex
SiftIndex siftIndex(size_t idx) const
Get the SIFT feature index corresponding to the measurement at idx
Definition: dataset.h:235
gtsam::SfmData::add_track
void add_track(const SfmTrack &t)
Add a track to SfmData.
Definition: dataset.h:272
gtsam::SfmData::tracks
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: dataset.h:255
gtsam::SfmData::number_tracks
size_t number_tracks() const
The number of reconstructed 3D points.
Definition: dataset.h:260
gtsam::SfmData::camera
SfmCamera camera(size_t idx) const
The camera pose at frame index idx
Definition: dataset.h:264
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::parseVertexPose
boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag)
Parse TORO/G2O vertex "id x y yaw".
Definition: dataset.cpp:168
gtsam::load2D_robust
GraphAndValues load2D_robust(const string &filename, const noiseModel::Base::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:575
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::NoiseFormatCOV
@ NoiseFormatCOV
Covariance matrix C11, C12, C13, C22, C23, C33.
Definition: dataset.h:67
gtsam::findExampleDataFile
string findExampleDataFile(const string &name)
Find the full path to an example dataset distributed with gtsam.
Definition: dataset.cpp:65
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::KernelFunctionType
KernelFunctionType
Robust kernel type to wrap around quadratic noise model.
Definition: dataset.h:72
gtsam::readG2o
GraphAndValues readG2o(const string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:615
gtsam::writeBAL
bool writeBAL(const string &filename, SfmData &data)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
Definition: dataset.cpp:1140
gtsam::NoiseFormat
NoiseFormat
Indicates how noise parameters are stored in file.
Definition: dataset.h:63
NoiseModel.h
Values.h
A non-templated config holding any types of Manifold-group elements.
Cal3Bundler.h
Calibration used by Bundler.
gtsam::createRewrittenFileName
string createRewrittenFileName(const string &name)
Creates a temporary file name that needs to be ignored in .gitingnore for checking read-write opratio...
Definition: dataset.cpp:100
gtsam::GraphAndValues
std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > GraphAndValues
Return type for load functions, which return a graph and initial values.
Definition: dataset.h:141
gtsam::IndexedPose
std::pair< size_t, Pose2 > IndexedPose
Return type for auxiliary functions.
Definition: dataset.h:109
gtsam::SfmTrack::measurements
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition: dataset.h:223
gtsam::SfmTrack::b
float b
RGB color of the 3D point.
Definition: dataset.h:222
gtsam::NoiseFormatAUTO
@ NoiseFormatAUTO
Try to guess covariance matrix layout.
Definition: dataset.h:68
gtsam::parseFactors
GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr > parseFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0)
Parse BetweenFactors in a line-based text format (like g2o) into a vector of shared pointers.
gtsam::save2D
void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const string &filename)
save 2d graph
Definition: dataset.cpp:582
gtsam::parseVertexLandmark
boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const string &tag)
Parse G2O landmark vertex "id x y".
Definition: dataset.cpp:188