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 
25 #include <gtsam/sfm/SfmData.h>
28 #include <gtsam/geometry/Pose2.h>
29 #include <gtsam/geometry/Pose3.h>
31 #include <gtsam/nonlinear/Values.h>
33 #include <gtsam/base/Testable.h>
34 #include <gtsam/base/types.h>
35 
36 #include <string>
37 #include <utility> // for pair
38 #include <vector>
39 #include <iosfwd>
40 #include <map>
41 #include <optional>
42 
43 namespace gtsam {
44 
56 GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
57 
62 GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
63 
71 };
72 
76 };
77 
84 template <typename T>
85 GTSAM_EXPORT std::map<size_t, T> parseVariables(const std::string &filename,
86  size_t maxIndex = 0);
87 
94 template <typename T>
95 GTSAM_EXPORT std::vector<BinaryMeasurement<T>>
96 parseMeasurements(const std::string &filename,
97  const noiseModel::Diagonal::shared_ptr &model = nullptr,
98  size_t maxIndex = 0);
99 
104 template <typename T>
105 GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr>
106 parseFactors(const std::string &filename,
107  const noiseModel::Diagonal::shared_ptr &model = nullptr,
108  size_t maxIndex = 0);
109 
111 typedef std::pair<size_t, Pose2> IndexedPose;
112 typedef std::pair<size_t, Point2> IndexedLandmark;
113 typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;
114 
120 GTSAM_EXPORT std::optional<IndexedPose> parseVertexPose(std::istream& is,
121  const std::string& tag);
122 
128 GTSAM_EXPORT std::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
129  const std::string& tag);
130 
136 GTSAM_EXPORT std::optional<IndexedEdge> parseEdge(std::istream& is,
137  const std::string& tag);
138 
142 using GraphAndValues =
143  std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;
144 
152 GTSAM_EXPORT GraphAndValues load2D(
153  std::pair<std::string, SharedNoiseModel> dataset, size_t maxIndex = 0,
154  bool addNoise = false,
155  bool smart = true, //
156  NoiseFormat noiseFormat = NoiseFormatAUTO,
157  KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
158 
170 GTSAM_EXPORT GraphAndValues
171 load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
172  size_t maxIndex = 0, bool addNoise = false, bool smart = true,
173  NoiseFormat noiseFormat = NoiseFormatAUTO, //
174  KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
175 
177 GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
178  const Values& config, const noiseModel::Diagonal::shared_ptr model,
179  const std::string& filename);
180 
189 GTSAM_EXPORT GraphAndValues
190 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 
211 // Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
212 using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
213 GTSAM_EXPORT BetweenFactorPose2s
214 parse2DFactors(const std::string &filename,
215  const noiseModel::Diagonal::shared_ptr &model = nullptr,
216  size_t maxIndex = 0);
217 
218 using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
219 GTSAM_EXPORT BetweenFactorPose3s
220 parse3DFactors(const std::string &filename,
221  const noiseModel::Diagonal::shared_ptr &model = nullptr,
222  size_t maxIndex = 0);
223 
224 using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
225 using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
226 using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
227 } // namespace gtsam
std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > GraphAndValues
Definition: dataset.h:143
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure. ...
Definition: dataset.cpp:636
Typedefs for easier changing of types.
default: toro-style order, but covariance matrix !
Definition: dataset.h:68
std::vector< BinaryMeasurement< Unit3 > > BinaryMeasurementsUnit3
Definition: dataset.h:224
Concept check for values that can be used in unit tests.
std::vector< BetweenFactor< Pose3 >::shared_ptr > BetweenFactorPose3s
Definition: dataset.h:218
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
KernelFunctionType
Robust kernel type to wrap around quadratic noise model.
Definition: dataset.h:74
noiseModel::Diagonal::shared_ptr model
std::vector< BinaryMeasurement< Point3 > > BinaryMeasurementsPoint3
Definition: dataset.h:225
BetweenFactorPose3s parse3DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:954
std::pair< size_t, Pose2 > IndexedPose
Return type for auxiliary functions.
Definition: dataset.h:111
BetweenFactorPose2s parse2DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:948
Try to guess covariance matrix layout.
Definition: dataset.h:70
NonlinearFactorGraph graph
NoiseFormat
Indicates how noise parameters are stored in file.
Definition: dataset.h:65
Base class for all pinhole cameras.
std::pair< std::pair< size_t, size_t >, Pose2 > IndexedEdge
Definition: dataset.h:113
static std::map< timestep_t, std::vector< stereo_meas_t > > dataset
Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
Definition: dataset.h:67
std::optional< IndexedEdge > parseEdge(std::istream &is, const std::string &tag)
Definition: dataset.cpp:299
GraphAndValues load3D(const std::string &filename)
Load TORO 3D Graph.
Definition: dataset.cpp:922
Data structure for dealing with Structure from Motion data.
std::pair< size_t, Point2 > IndexedLandmark
Definition: dataset.h:112
GraphAndValues load2D(const std::string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Definition: dataset.cpp:505
std::vector< BinaryMeasurement< Rot3 > > BinaryMeasurementsRot3
Definition: dataset.h:226
void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const std::string &filename)
Definition: dataset.cpp:588
GraphAndValues readG2o(const std::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:621
traits
Definition: chartTesting.h:28
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
Covariance matrix C11, C12, C13, C22, C23, C33.
Definition: dataset.h:69
Information matrix I11, I12, I13, I22, I23, I33.
Definition: dataset.h:66
GTSAM_EXPORT std::map< size_t, T > parseVariables(const std::string &filename, size_t maxIndex=0)
std::optional< IndexedLandmark > parseVertexLandmark(std::istream &is, const std::string &tag)
Definition: dataset.cpp:193
Annotation for function names.
Definition: attr.h:48
std::vector< BetweenFactor< Pose2 >::shared_ptr > BetweenFactorPose2s
Definition: dataset.h:212
2D Pose
GTSAM_EXPORT std::string createRewrittenFileName(const std::string &name)
Definition: dataset.cpp:105
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
std::optional< IndexedPose > parseVertexPose(std::istream &is, const std::string &tag)
Definition: dataset.cpp:173
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
3D Pose
Calibration used by Bundler.
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)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose2 > > parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:396


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