Go to the documentation of this file.
52 #if defined(__GNUC__) && (__GNUC__ == 7)
53 #include <experimental/filesystem>
54 namespace fs = std::experimental::filesystem;
57 namespace fs = std::filesystem;
65 #define LINESIZE 81920
72 std::vector<std::string> rootsToSearch;
75 rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR);
76 rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR);
79 std::vector<std::string> namesToSearch;
80 namesToSearch.push_back(
name);
81 namesToSearch.push_back(
name +
".graph");
82 namesToSearch.push_back(
name +
".txt");
83 namesToSearch.push_back(
name +
".out");
84 namesToSearch.push_back(
name +
".xml");
85 namesToSearch.push_back(
name +
".g2o");
88 for (
const fs::path root : rootsToSearch) {
90 if (fs::is_regular_file(root /
name))
91 return (root /
name).string();
96 throw std::invalid_argument(
97 "gtsam::findExampleDataFile could not find a matching "
98 "file in\n" GTSAM_SOURCE_TREE_DATASET_DIR
99 " or\n" GTSAM_INSTALLED_DATASET_DIR
" named\n" +
108 throw std::invalid_argument(
109 "gtsam::createRewrittenFileName could not find a matching file in\n" +
115 fs::path(
p.stem().string() +
"-rewritten.txt");
117 return newpath.string();
122 template <
typename T>
124 std::function<std::optional<T>(std::istream &is,
const std::string &tag)>;
129 template <
typename T>
133 throw std::invalid_argument(
"parse: can not find file " +
filename);
143 template <
typename T>
146 std::map<size_t, T>
result;
148 if (
auto t =
parse(is, tag)) {
149 if (!maxIndex ||
t->first <= maxIndex)
160 template <
typename T>
164 if (
auto t =
parse(is, tag))
173 std::optional<IndexedPose>
parseVertexPose(std::istream &is,
const std::string &tag) {
174 if ((tag ==
"VERTEX2") || (tag ==
"VERTEX_SE2") || (tag ==
"VERTEX")) {
177 if (!(is >>
id >>
x >>
y >> yaw)) {
178 throw std::runtime_error(
"parseVertexPose encountered malformed line");
188 const std::string &
filename,
size_t maxIndex) {
194 const std::string &tag) {
195 if (tag ==
"VERTEX_XY") {
198 if (!(is >>
id >>
x >>
y)) {
199 throw std::runtime_error(
200 "parseVertexLandmark encountered malformed line");
210 const std::string &
filename,
size_t maxIndex) {
221 if (
v(0) != 0.0 &&
v(1) == 0.0 &&
v(2) != 0.0 &&
222 v(3) != 0.0 &&
v(4) == 0.0 &&
v(5) == 0.0) {
224 }
else if (
v(0) != 0.0 &&
v(1) == 0.0 &&
v(2) == 0.0 &&
225 v(3) != 0.0 &&
v(4) == 0.0 &&
v(5) != 0.0) {
228 throw std::invalid_argument(
229 "load2D: unrecognized covariance matrix format in dataset file. "
230 "Please specify the noise format.");
236 switch (noiseFormat) {
242 if (
v(0) == 0.0 ||
v(3) == 0.0 ||
v(5) == 0.0)
243 throw std::runtime_error(
244 "load2D::readNoiseModel looks like this is not G2O matrix order");
245 M <<
v(0),
v(1),
v(2),
v(1),
v(3),
v(4),
v(2),
v(4),
v(5);
254 if (
v(0) == 0.0 ||
v(2) == 0.0 ||
v(3) == 0.0)
255 throw std::invalid_argument(
256 "load2D::readNoiseModel looks like this is not TORO matrix order");
257 M <<
v(0),
v(1),
v(4),
v(1),
v(2),
v(5),
v(4),
v(5),
v(3);
260 throw std::runtime_error(
"load2D: invalid noise format");
266 switch (noiseFormat) {
278 throw std::invalid_argument(
"load2D: invalid noise format");
281 switch (kernelFunctionType) {
294 throw std::invalid_argument(
"load2D: invalid kernel function type");
299 std::optional<IndexedEdge>
parseEdge(std::istream &is,
const std::string &tag) {
300 if ((tag ==
"EDGE2") || (tag ==
"EDGE") || (tag ==
"EDGE_SE2") ||
301 (tag ==
"ODOMETRY")) {
305 if (!(is >> id1 >> id2 >>
x >>
y >> yaw)) {
306 throw std::runtime_error(
"parseEdge encountered malformed line");
326 typename std::optional<typename BetweenFactor<T>::shared_ptr>
329 return std::make_shared<BetweenFactor<T>>(
330 m->key1(),
m->key2(),
m->measured(),
m->noiseModel());
352 std::optional<BinaryMeasurement<Pose2>>
operator()(std::istream &is,
353 const std::string &tag) {
360 is >>
v(0) >>
v(1) >>
v(2) >>
v(3) >>
v(4) >>
v(5);
364 std::tie(id1, id2) = edge->first;
365 if (maxIndex && (id1 > maxIndex || id2 > maxIndex))
371 pose =
pose.retract(sampler->sample());
384 auto noise = std::dynamic_pointer_cast<noiseModel::Diagonal>(
model);
386 throw std::invalid_argument(
"gtsam::load: invalid noise model for adding noise"
387 "(current version assumes diagonal noise model)!");
388 return std::shared_ptr<Sampler>(
new Sampler(noise));
395 std::vector<BinaryMeasurement<Pose2>>
402 return parseToVector<BinaryMeasurement<Pose2>>(
filename,
parse);
411 std::dynamic_pointer_cast<noiseModel::Gaussian>(
p.noiseModel());
413 throw std::invalid_argument(
414 "parseMeasurements<Rot2> can only convert Pose2 measurements "
415 "with Gaussian noise models.");
416 const Matrix3
M = gaussian->covariance();
423 std::vector<BinaryMeasurement<Rot2>>
427 auto poses = parseMeasurements<Pose2>(
filename,
model, maxIndex);
428 std::vector<BinaryMeasurement<Rot2>>
result;
429 result.reserve(poses.size());
430 for (
const auto &
p : poses)
439 std::vector<BetweenFactor<Pose2>::shared_ptr>
446 return parseToVector<BetweenFactor<Pose2>::shared_ptr>(
filename,
parse);
457 std::optional<BinaryMeasurement<BearingRange2D>>
461 double bearing,
range, bearing_std, range_std;
464 is >> bearing >>
range >> bearing_std >> range_std;
465 }
else if (tag ==
"LANDMARK") {
470 is >> lmx >> lmy >>
v1 >>
v2 >>
v3;
473 bearing =
atan2(lmy, lmx);
480 bearing_std =
sqrt(
v1 / 10.0);
492 if (maxIndex && id1 > maxIndex)
497 (
Vector(2) << bearing_std, range_std).finished());
506 size_t maxIndex,
bool addNoise,
bool smart,
511 auto initial = std::make_shared<Values>();
514 if (!maxIndex || indexedPose->first <= maxIndex)
515 initial->insert(indexedPose->first, indexedPose->second);
517 if (!maxIndex || indexedLandmark->first <= maxIndex)
518 initial->insert(
L(indexedLandmark->first), indexedLandmark->second);
525 auto graph = std::make_shared<NonlinearFactorGraph>();
530 kernelFunctionType,
model});
538 if (
auto f = parseBetweenFactor(is, tag)) {
547 }
else if (
auto m = parseBearingRange(is, tag)) {
573 size_t maxIndex,
bool addNoise,
bool smart,
577 noiseFormat, kernelFunctionType);
597 stream <<
"VERTEX2 " << key_pose.first <<
" " <<
pose.
x() <<
" " <<
pose.
y()
598 <<
" " <<
pose.theta() << endl;
606 auto factor = std::dynamic_pointer_cast<BetweenFactor<Pose2>>(
f);
611 stream <<
"EDGE2 " << factor->key<2>() <<
" " << factor->key<1>() <<
" "
613 << RR(0, 0) <<
" " << RR(0, 1) <<
" " << RR(1, 1) <<
" " << RR(2, 2)
614 <<
" " << RR(0, 2) <<
" " << RR(1, 2) << endl;
628 bool addNoise =
false;
646 stream <<
"VERTEX_SE2 " << index(pair.first) <<
" " <<
pose.
x() <<
" "
647 <<
pose.
y() <<
" " <<
pose.theta() << endl;
655 stream <<
"VERTEX_SE3:QUAT " << index(pair.first) <<
" " <<
t.x() <<
" "
656 <<
t.y() <<
" " <<
t.z() <<
" " <<
q.x() <<
" " <<
q.y() <<
" "
657 <<
q.z() <<
" " <<
q.w() << endl;
663 stream <<
"VERTEX_XY " << index(pair.first) <<
" " <<
point.x() <<
" "
664 <<
point.y() << endl;
670 stream <<
"VERTEX_TRACKXYZ " << index(pair.first) <<
" " <<
point.x()
671 <<
" " <<
point.y() <<
" " <<
point.z() << endl;
675 for (
const auto &factor_ :
graph) {
676 auto factor = std::dynamic_pointer_cast<BetweenFactor<Pose2>>(factor_);
680 std::dynamic_pointer_cast<noiseModel::Gaussian>(
model);
681 if (!gaussianModel) {
682 model->print(
"model\n");
683 throw std::invalid_argument(
"writeG2o: invalid noise model!");
685 Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R();
687 stream <<
"EDGE_SE2 " << index(factor->key<1>()) <<
" "
688 << index(factor->key<2>()) <<
" " <<
pose.
x() <<
" " <<
pose.
y()
689 <<
" " <<
pose.theta();
690 for (
size_t i = 0;
i < 3;
i++) {
691 for (
size_t j =
i;
j < 3;
j++) {
698 auto factor3D = std::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor_);
703 std::shared_ptr<noiseModel::Gaussian> gaussianModel =
704 std::dynamic_pointer_cast<noiseModel::Gaussian>(
model);
705 if (!gaussianModel) {
706 model->print(
"model\n");
707 throw std::invalid_argument(
"writeG2o: invalid noise model!");
709 Matrix6 Info = gaussianModel->R().transpose() * gaussianModel->R();
710 const Pose3 pose3D = factor3D->measured();
713 stream <<
"EDGE_SE3:QUAT " << index(factor3D->key<1>()) <<
" "
714 << index(factor3D->key<2>()) <<
" " <<
p.x() <<
" " <<
p.y() <<
" "
715 <<
p.z() <<
" " <<
q.x() <<
" " <<
q.y() <<
" " <<
q.z() <<
" "
719 Matrix6 InfoG2o = I_6x6;
720 InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3);
721 InfoG2o.block<3, 3>(3, 3) = Info.block<3, 3>(0, 0);
722 InfoG2o.block<3, 3>(0, 3) = Info.block<3, 3>(3, 0);
723 InfoG2o.block<3, 3>(3, 0) = Info.block<3, 3>(0, 3);
725 for (
size_t i = 0;
i < 6;
i++) {
726 for (
size_t j =
i;
j < 6;
j++) {
740 is >>
x >>
y >>
z >>
w;
741 const double norm =
sqrt(
w *
w +
x *
x +
y *
y +
z *
z),
f = 1.0 / norm;
749 double yaw, pitch, roll;
750 is >> roll >> pitch >> yaw;
757 const std::string &tag) {
758 if (tag ==
"VERTEX3") {
762 is >>
id >>
x >>
y >>
z >>
R;
763 return std::make_pair(
id,
Pose3(
R, {
x,
y,
z}));
764 }
else if (tag ==
"VERTEX_SE3:QUAT") {
768 is >>
id >>
x >>
y >>
z >>
q;
769 return std::make_pair(
id,
Pose3(
q, {
x,
y,
z}));
776 const std::string &
filename,
size_t maxIndex) {
782 const std::string &tag) {
783 if (tag ==
"VERTEX_TRACKXYZ") {
786 is >>
id >>
x >>
y >>
z;
787 return std::make_pair(
id,
Point3(
x,
y,
z));
794 const std::string &
filename,
size_t maxIndex) {
801 for (
size_t i = 0;
i < 6;
i++)
802 for (
size_t j =
i;
j < 6;
j++) {
817 std::optional<BinaryMeasurement<Pose3>>
operator()(std::istream &is,
818 const std::string &tag) {
819 if (tag !=
"EDGE3" && tag !=
"EDGE_SE3:QUAT")
825 if (maxIndex && (id1 > maxIndex || id2 > maxIndex))
829 if (tag ==
"EDGE3") {
832 is >>
x >>
y >>
z >>
R >>
m;
836 T12 = T12.
retract(sampler->sample());
840 }
else if (tag ==
"EDGE_SE3:QUAT") {
843 is >>
x >>
y >>
z >>
q >>
m;
848 T12 = T12.
retract(sampler->sample());
852 mgtsam.block<3, 3>(0, 0) =
m.block<3, 3>(3, 3);
853 mgtsam.block<3, 3>(3, 3) =
m.block<3, 3>(0, 0);
854 mgtsam.block<3, 3>(3, 0) =
m.block<3, 3>(0, 3);
855 mgtsam.block<3, 3>(0, 3) =
m.block<3, 3>(3, 0);
869 std::vector<BinaryMeasurement<Pose3>>
875 return parseToVector<BinaryMeasurement<Pose3>>(
filename,
parse);
884 std::dynamic_pointer_cast<noiseModel::Gaussian>(
p.noiseModel());
886 throw std::invalid_argument(
887 "parseMeasurements<Rot3> can only convert Pose3 measurements "
888 "with Gaussian noise models.");
889 const Matrix6
M = gaussian->covariance();
891 p.key1(),
p.key2(),
p.measured().rotation(),
897 std::vector<BinaryMeasurement<Rot3>>
901 auto poses = parseMeasurements<Pose3>(
filename,
model, maxIndex);
902 std::vector<BinaryMeasurement<Rot3>>
result;
903 result.reserve(poses.size());
904 for (
const auto &
p : poses)
913 std::vector<BetweenFactor<Pose3>::shared_ptr>
918 return parseToVector<BetweenFactor<Pose3>::shared_ptr>(
filename,
parse);
923 auto graph = std::make_shared<NonlinearFactorGraph>();
924 auto initial = std::make_shared<Values>();
933 initial->insert(indexedPose->first, indexedPose->second);
935 initial->insert(
L(indexedLandmark->first), indexedLandmark->second);
936 }
else if (
auto factor = parseFactor(is, tag)) {
const Symbol key1('v', 1)
std::shared_ptr< Sampler > sampler
@ NoiseFormatTORO
Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
std::optional< typename BetweenFactor< T >::shared_ptr > operator()(std::istream &is, const std::string &tag)
Eigen::Quaternion< double, Eigen::DontAlign > Quaternion
GTSAM_EXPORT std::string createRewrittenFileName(const std::string &name)
Annotation for function names.
std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > GraphAndValues
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
std::pair< size_t, Pose2 > IndexedPose
Return type for auxiliary functions.
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
@ NoiseFormatGRAPH
default: toro-style order, but covariance matrix !
static Y add(const Y &y1, const Y &y2)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
typedef and functions to augment Eigen's VectorXd
Array< double, 1, 3 > e(1./3., 0.5, 2.)
@ NoiseFormatCOV
Covariance matrix C11, C12, C13, C22, C23, C33.
std::pair< size_t, Point2 > IndexedLandmark
Typedefs for easier changing of types.
std::shared_ptr< Base > shared_ptr
@ NoiseFormatG2O
Information matrix I11, I12, I13, I22, I23, I33.
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
typedef and functions to augment Eigen's MatrixXd
BetweenFactorPose3s parse3DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
static std::vector< T > parseToVector(const std::string &filename, Parser< T > parse)
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...
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
GTSAM_EXPORT std::map< size_t, Pose2 > parseVariables< Pose2 >(const std::string &filename, size_t maxIndex)
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={})
static SharedNoiseModel createNoiseModel(const Vector6 &v, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
3D rotation represented as a rotation matrix or quaternion
Double_ range(const Point2_ &p, const Point2_ &q)
GTSAM_EXPORT std::map< size_t, Point2 > parseVariables< Point2 >(const std::string &filename, size_t maxIndex)
std::pair< std::pair< size_t, size_t >, Pose2 > IndexedEdge
std::optional< BinaryMeasurement< Pose3 > > operator()(std::istream &is, const std::string &tag)
static const Similarity3 id
NoiseFormat
Indicates how noise parameters are stored in file.
const Symbol key2('v', 2)
utility functions for loading datasets
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
EIGEN_DEVICE_FUNC const Scalar & q
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
std::shared_ptr< Sampler > createSampler(const SharedNoiseModel &model)
std::function< std::optional< T >(std::istream &is, const std::string &tag)> Parser
Pose3 inverse() const
inverse transformation with derivatives
static shared_ptr Information(const Matrix &M, bool smart=true)
std::optional< BinaryMeasurement< Pose2 > > operator()(std::istream &is, const std::string &tag)
std::optional< IndexedPose > parseVertexPose(std::istream &is, const std::string &tag)
gtsam::Quaternion toQuaternion() const
ParseFactor(const ParseMeasurement< T > &parent)
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
@ KernelFunctionTypeHUBER
KernelFunctionType
Robust kernel type to wrap around quadratic noise model.
std::optional< BinaryMeasurement< BearingRange2D > > operator()(std::istream &is, const std::string &tag)
noiseModel::Base::shared_ptr SharedNoiseModel
Matrix2 transpose() const
noiseModel::Diagonal::shared_ptr model
std::ofstream out("Result.txt")
BetweenFactorPose2s parse2DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
const gtsam::Symbol key('X', 0)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Non-linear factor base classes.
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
The quaternion class used to represent 3D orientations and rotations.
GraphAndValues load2D_robust(const std::string &filename, const noiseModel::Base::shared_ptr &model, size_t maxIndex)
def parse(input_path, output_path, quiet=False, generate_xml_flag=True)
std::uint64_t index() const
static void parseLines(const std::string &filename, Parser< T > parse)
GraphAndValues load2D(const std::string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Base class and basic functions for Lie types.
std::vector< BetweenFactor< Pose3 >::shared_ptr > BetweenFactorPose3s
@ KernelFunctionTypeTUKEY
BearingRange< Pose2, Point2 > BearingRange2D
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const std::string &filename)
std::optional< std::pair< size_t, Point3 > > parseVertexPoint3(std::istream &is, const std::string &tag)
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
GTSAM_EXPORT std::map< size_t, Point3 > parseVariables< Point3 >(const std::string &filename, size_t maxIndex)
KernelFunctionType kernelFunctionType
std::shared_ptr< Diagonal > shared_ptr
std::optional< std::pair< size_t, Pose3 > > parseVertexPose3(std::istream &is, const std::string &tag)
Array< int, Dynamic, 1 > v
std::shared_ptr< Sampler > sampler
GTSAM_EXPORT std::vector< BetweenFactor< Pose3 >::shared_ptr > parseFactors< Pose3 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
std::vector< BetweenFactor< Pose2 >::shared_ptr > BetweenFactorPose2s
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
static std::map< timestep_t, std::vector< stereo_meas_t > > dataset
std::optional< IndexedEdge > parseEdge(std::istream &is, const std::string &tag)
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
sampling from a NoiseModel
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
GTSAM_EXPORT std::map< size_t, Pose3 > parseVariables< Pose3 >(const std::string &filename, size_t maxIndex)
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
std::map< size_t, T > parseToMap(const std::string &filename, Parser< std::pair< size_t, T >> parse, size_t maxIndex)
GraphAndValues load3D(const std::string &filename)
Load TORO 3D Graph.
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose2 > > parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
static BinaryMeasurement< Rot3 > convert(const BetweenFactor< Pose3 >::shared_ptr &f)
@ NoiseFormatAUTO
Try to guess covariance matrix layout.
std::optional< IndexedLandmark > parseVertexLandmark(std::istream &is, const std::string &tag)
Jet< T, N > sqrt(const Jet< T, N > &f)
The base class for any variable that can be optimized or used in a factor.
Rot2 R(Rot2::fromAngle(0.1))
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
GTSAM_EXPORT std::vector< BetweenFactor< Pose2 >::shared_ptr > parseFactors< Pose2 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
a single factor contains both the bearing and the range to prevent handle to pair bearing and range f...
Matrix< RealScalar, Dynamic, Dynamic > M
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:22