gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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>
34#include <gtsam/base/Testable.h>
35#include <gtsam/base/types.h>
36
37#include <boost/smart_ptr/shared_ptr.hpp>
38#include <string>
39#include <utility> // for pair
40#include <vector>
41#include <iosfwd>
42#include <map>
43
44namespace gtsam {
45
57GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
58
63GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
64
73
76 KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
77};
78
85template <typename T>
86GTSAM_EXPORT std::map<size_t, T> parseVariables(const std::string &filename,
87 size_t maxIndex = 0);
88
95template <typename T>
96GTSAM_EXPORT std::vector<BinaryMeasurement<T>>
97parseMeasurements(const std::string &filename,
98 const noiseModel::Diagonal::shared_ptr &model = nullptr,
99 size_t maxIndex = 0);
100
105template <typename T>
106GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr>
107parseFactors(const std::string &filename,
108 const noiseModel::Diagonal::shared_ptr &model = nullptr,
109 size_t maxIndex = 0);
110
112typedef std::pair<size_t, Pose2> IndexedPose;
113typedef std::pair<size_t, Point2> IndexedLandmark;
114typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;
115
121GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
122 const std::string& tag);
123
129GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
130 const std::string& tag);
131
137GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
138 const std::string& tag);
139
144 std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;
145
153GTSAM_EXPORT GraphAndValues load2D(
154 std::pair<std::string, SharedNoiseModel> dataset, size_t maxIndex = 0,
155 bool addNoise = false,
156 bool smart = true, //
157 NoiseFormat noiseFormat = NoiseFormatAUTO,
158 KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
159
171GTSAM_EXPORT GraphAndValues
172load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
173 size_t maxIndex = 0, bool addNoise = false, bool smart = true,
174 NoiseFormat noiseFormat = NoiseFormatAUTO, //
175 KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
176
178GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
179 const Values& config, const noiseModel::Diagonal::shared_ptr model,
180 const std::string& filename);
181
190GTSAM_EXPORT GraphAndValues
191readG2o(const std::string& g2oFile, const bool is3D = false,
192 KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
193
206GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
207 const Values& estimate, const std::string& filename);
208
210GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
211
212// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
213using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
214GTSAM_EXPORT BetweenFactorPose2s
215parse2DFactors(const std::string &filename,
216 const noiseModel::Diagonal::shared_ptr &model = nullptr,
217 size_t maxIndex = 0);
218
219using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
220GTSAM_EXPORT BetweenFactorPose3s
221parse3DFactors(const std::string &filename,
222 const noiseModel::Diagonal::shared_ptr &model = nullptr,
223 size_t maxIndex = 0);
224
225using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
226using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
227using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
228
229#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
230inline boost::optional<IndexedPose> GTSAM_DEPRECATED
231parseVertex(std::istream& is, const std::string& tag) {
232 return parseVertexPose(is, tag);
233}
234
235GTSAM_EXPORT std::map<size_t, Pose3> GTSAM_DEPRECATED
236parse3DPoses(const std::string& filename, size_t maxIndex = 0);
237
238GTSAM_EXPORT std::map<size_t, Point3> GTSAM_DEPRECATED
239parse3DLandmarks(const std::string& filename, size_t maxIndex = 0);
240
241GTSAM_EXPORT GraphAndValues GTSAM_DEPRECATED
242load2D_robust(const std::string& filename,
243 const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
244#endif
245} // namespace gtsam
Concept check for values that can be used in unit tests.
Convenience functions for serializing data structures via boost.serialization.
Typedefs for easier changing of types.
Calibration used by Bundler.
Base class for all pinhole cameras.
2D Pose
3D Pose
Factor Graph consisting of non-linear factors.
A non-templated config holding any types of Manifold-group elements.
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
Data structure for dealing with Structure from Motion data.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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:502
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.
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:632
std::pair< size_t, Pose2 > IndexedPose
Return type for auxiliary functions.
Definition dataset.h:112
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:617
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.
KernelFunctionType
Robust kernel type to wrap around quadratic noise model.
Definition dataset.h:75
NoiseFormat
Indicates how noise parameters are stored in file.
Definition dataset.h:66
@ NoiseFormatGRAPH
default: toro-style order, but covariance matrix !
Definition dataset.h:69
@ NoiseFormatAUTO
Try to guess covariance matrix layout.
Definition dataset.h:71
@ NoiseFormatTORO
Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
Definition dataset.h:68
@ NoiseFormatCOV
Covariance matrix C11, C12, C13, C22, C23, C33.
Definition dataset.h:70
@ NoiseFormatG2O
Information matrix I11, I12, I13, I22, I23, I33.
Definition dataset.h:67
boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const string &tag)
Parse G2O landmark vertex "id x y".
Definition dataset.cpp:187
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag)
Parse TORO/G2O edge "id1 id2 x y yaw".
Definition dataset.cpp:293
std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > GraphAndValues
Return type for load functions, which return a graph and initial values.
Definition dataset.h:144
boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag)
Parse TORO/G2O vertex "id x y yaw".
Definition dataset.cpp:167
GraphAndValues load3D(const string &filename)
Load TORO 3D Graph.
Definition dataset.cpp:918
void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const string &filename)
save 2d graph
Definition dataset.cpp:584
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:99
string findExampleDataFile(const string &name)
Find the full path to an example dataset distributed with gtsam.
Definition dataset.cpp:62
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
Definition NonlinearFactorGraph.h:55
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65