Do not use std...
Frank Dellaert
parent
22e14228e2
commit
f08f8afdd0
|
|
@ -47,30 +47,35 @@
|
|||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#if defined(__GNUC__) && (__GNUC__ == 7)
|
||||
#include <experimental/filesystem>
|
||||
#else
|
||||
#include <filesystem>
|
||||
#endif
|
||||
using namespace std;
|
||||
|
||||
namespace fs = std::filesystem;
|
||||
using gtsam::symbol_shorthand::L;
|
||||
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
|
||||
#define LINESIZE 81920
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
string findExampleDataFile(const string &name) {
|
||||
std::string findExampleDataFile(const std::string &name) {
|
||||
// Search source tree and installed location
|
||||
vector<string> rootsToSearch;
|
||||
std::vector<std::string> rootsToSearch;
|
||||
|
||||
// Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt
|
||||
rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR);
|
||||
rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR);
|
||||
|
||||
// Search for filename as given, and with .graph and .txt extensions
|
||||
vector<string> namesToSearch;
|
||||
std::vector<std::string> namesToSearch;
|
||||
namesToSearch.push_back(name);
|
||||
namesToSearch.push_back(name + ".graph");
|
||||
namesToSearch.push_back(name + ".txt");
|
||||
|
|
@ -87,7 +92,7 @@ string findExampleDataFile(const string &name) {
|
|||
}
|
||||
|
||||
// If we did not return already, then we did not find the file
|
||||
throw invalid_argument(
|
||||
throw std::invalid_argument(
|
||||
"gtsam::findExampleDataFile could not find a matching "
|
||||
"file in\n" GTSAM_SOURCE_TREE_DATASET_DIR
|
||||
" or\n" GTSAM_INSTALLED_DATASET_DIR " named\n" +
|
||||
|
|
@ -96,10 +101,10 @@ string findExampleDataFile(const string &name) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string createRewrittenFileName(const string &name) {
|
||||
std::string createRewrittenFileName(const std::string &name) {
|
||||
// Search source tree and installed location
|
||||
if (!exists(fs::path(name))) {
|
||||
throw invalid_argument(
|
||||
throw std::invalid_argument(
|
||||
"gtsam::createRewrittenFileName could not find a matching file in\n" +
|
||||
name);
|
||||
}
|
||||
|
|
@ -115,17 +120,17 @@ string createRewrittenFileName(const string &name) {
|
|||
// Type for parser functions used in parseLines below.
|
||||
template <typename T>
|
||||
using Parser =
|
||||
std::function<std::optional<T>(istream &is, const string &tag)>;
|
||||
std::function<std::optional<T>(std::istream &is, const std::string &tag)>;
|
||||
|
||||
// Parse a file by calling the parse(is, tag) function for every line.
|
||||
// The result of calling the function is ignored, so typically parse function
|
||||
// works via a side effect, like adding a factor into a graph etc.
|
||||
template <typename T>
|
||||
static void parseLines(const string &filename, Parser<T> parse) {
|
||||
ifstream is(filename.c_str());
|
||||
static void parseLines(const std::string &filename, Parser<T> parse) {
|
||||
std::ifstream is(filename.c_str());
|
||||
if (!is)
|
||||
throw invalid_argument("parse: can not find file " + filename);
|
||||
string tag;
|
||||
throw std::invalid_argument("parse: can not find file " + filename);
|
||||
std::string tag;
|
||||
while (is >> tag) {
|
||||
parse(is, tag); // ignore return value
|
||||
is.ignore(LINESIZE, '\n');
|
||||
|
|
@ -135,10 +140,10 @@ static void parseLines(const string &filename, Parser<T> parse) {
|
|||
/* ************************************************************************* */
|
||||
// Parse types T into a size_t-indexed map
|
||||
template <typename T>
|
||||
map<size_t, T> parseToMap(const string &filename, Parser<pair<size_t, T>> parse,
|
||||
std::map<size_t, T> parseToMap(const std::string &filename, Parser<std::pair<size_t, T>> parse,
|
||||
size_t maxIndex) {
|
||||
map<size_t, T> result;
|
||||
Parser<pair<size_t, T>> emplace = [&](istream &is, const string &tag) {
|
||||
std::map<size_t, T> result;
|
||||
Parser<std::pair<size_t, T>> emplace = [&](std::istream &is, const std::string &tag) {
|
||||
if (auto t = parse(is, tag)) {
|
||||
if (!maxIndex || t->first <= maxIndex)
|
||||
result.emplace(*t);
|
||||
|
|
@ -152,9 +157,9 @@ map<size_t, T> parseToMap(const string &filename, Parser<pair<size_t, T>> parse,
|
|||
/* ************************************************************************* */
|
||||
// Parse a file and push results on a vector
|
||||
template <typename T>
|
||||
static vector<T> parseToVector(const string &filename, Parser<T> parse) {
|
||||
vector<T> result;
|
||||
Parser<T> add = [&result, parse](istream &is, const string &tag) {
|
||||
static std::vector<T> parseToVector(const std::string &filename, Parser<T> parse) {
|
||||
std::vector<T> result;
|
||||
Parser<T> add = [&result, parse](std::istream &is, const std::string &tag) {
|
||||
if (auto t = parse(is, tag))
|
||||
result.push_back(*t);
|
||||
return std::nullopt;
|
||||
|
|
@ -164,7 +169,7 @@ static vector<T> parseToVector(const string &filename, Parser<T> parse) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::optional<IndexedPose> parseVertexPose(istream &is, const string &tag) {
|
||||
std::optional<IndexedPose> parseVertexPose(std::istream &is, const std::string &tag) {
|
||||
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
|
||||
size_t id;
|
||||
double x, y, yaw;
|
||||
|
|
@ -184,8 +189,8 @@ GTSAM_EXPORT std::map<size_t, Pose2> parseVariables<Pose2>(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::optional<IndexedLandmark> parseVertexLandmark(istream &is,
|
||||
const string &tag) {
|
||||
std::optional<IndexedLandmark> parseVertexLandmark(std::istream &is,
|
||||
const std::string &tag) {
|
||||
if (tag == "VERTEX_XY") {
|
||||
size_t id;
|
||||
double x, y;
|
||||
|
|
@ -234,7 +239,7 @@ static SharedNoiseModel createNoiseModel(
|
|||
// v(1)' v(3) v(4);
|
||||
// v(2)' v(4)' v(5) ]
|
||||
if (v(0) == 0.0 || v(3) == 0.0 || v(5) == 0.0)
|
||||
throw runtime_error(
|
||||
throw std::runtime_error(
|
||||
"load2D::readNoiseModel looks like this is not G2O matrix order");
|
||||
M << v(0), v(1), v(2), v(1), v(3), v(4), v(2), v(4), v(5);
|
||||
break;
|
||||
|
|
@ -246,12 +251,12 @@ static SharedNoiseModel createNoiseModel(
|
|||
// v(1)' v(2) v(5);
|
||||
// v(4)' v(5)' v(3) ]
|
||||
if (v(0) == 0.0 || v(2) == 0.0 || v(3) == 0.0)
|
||||
throw invalid_argument(
|
||||
throw std::invalid_argument(
|
||||
"load2D::readNoiseModel looks like this is not TORO matrix order");
|
||||
M << v(0), v(1), v(4), v(1), v(2), v(5), v(4), v(5), v(3);
|
||||
break;
|
||||
default:
|
||||
throw runtime_error("load2D: invalid noise format");
|
||||
throw std::runtime_error("load2D: invalid noise format");
|
||||
}
|
||||
|
||||
// Now, create a Gaussian noise model
|
||||
|
|
@ -269,7 +274,7 @@ static SharedNoiseModel createNoiseModel(
|
|||
model = noiseModel::Gaussian::Covariance(M, smart);
|
||||
break;
|
||||
default:
|
||||
throw invalid_argument("load2D: invalid noise format");
|
||||
throw std::invalid_argument("load2D: invalid noise format");
|
||||
}
|
||||
|
||||
switch (kernelFunctionType) {
|
||||
|
|
@ -285,12 +290,12 @@ static SharedNoiseModel createNoiseModel(
|
|||
noiseModel::mEstimator::Tukey::Create(4.6851), model);
|
||||
break;
|
||||
default:
|
||||
throw invalid_argument("load2D: invalid kernel function type");
|
||||
throw std::invalid_argument("load2D: invalid kernel function type");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::optional<IndexedEdge> parseEdge(istream &is, const string &tag) {
|
||||
std::optional<IndexedEdge> parseEdge(std::istream &is, const std::string &tag) {
|
||||
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") ||
|
||||
(tag == "ODOMETRY")) {
|
||||
|
||||
|
|
@ -318,7 +323,7 @@ template <typename T> struct ParseFactor : ParseMeasurement<T> {
|
|||
|
||||
// We parse a measurement then convert
|
||||
typename std::optional<typename BetweenFactor<T>::shared_ptr>
|
||||
operator()(istream &is, const string &tag) {
|
||||
operator()(std::istream &is, const std::string &tag) {
|
||||
if (auto m = ParseMeasurement<T>::operator()(is, tag))
|
||||
return std::make_shared<BetweenFactor<T>>(
|
||||
m->key1(), m->key2(), m->measured(), m->noiseModel());
|
||||
|
|
@ -343,8 +348,8 @@ template <> struct ParseMeasurement<Pose2> {
|
|||
SharedNoiseModel model;
|
||||
|
||||
// The actual parser
|
||||
std::optional<BinaryMeasurement<Pose2>> operator()(istream &is,
|
||||
const string &tag) {
|
||||
std::optional<BinaryMeasurement<Pose2>> operator()(std::istream &is,
|
||||
const std::string &tag) {
|
||||
auto edge = parseEdge(is, tag);
|
||||
if (!edge)
|
||||
return std::nullopt;
|
||||
|
|
@ -355,7 +360,7 @@ template <> struct ParseMeasurement<Pose2> {
|
|||
|
||||
// optional filter
|
||||
size_t id1, id2;
|
||||
tie(id1, id2) = edge->first;
|
||||
std::tie(id1, id2) = edge->first;
|
||||
if (maxIndex && (id1 > maxIndex || id2 > maxIndex))
|
||||
return std::nullopt;
|
||||
|
||||
|
|
@ -377,7 +382,7 @@ template <> struct ParseMeasurement<Pose2> {
|
|||
std::shared_ptr<Sampler> createSampler(const SharedNoiseModel &model) {
|
||||
auto noise = std::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (!noise)
|
||||
throw invalid_argument("gtsam::load: invalid noise model for adding noise"
|
||||
throw std::invalid_argument("gtsam::load: invalid noise model for adding noise"
|
||||
"(current version assumes diagonal noise model)!");
|
||||
return std::shared_ptr<Sampler>(new Sampler(noise));
|
||||
}
|
||||
|
|
@ -449,7 +454,7 @@ template <> struct ParseMeasurement<BearingRange2D> {
|
|||
|
||||
// The actual parser
|
||||
std::optional<BinaryMeasurement<BearingRange2D>>
|
||||
operator()(istream &is, const string &tag) {
|
||||
operator()(std::istream &is, const std::string &tag) {
|
||||
if (tag != "BR" && tag != "LANDMARK")
|
||||
return std::nullopt;
|
||||
|
||||
|
|
@ -499,14 +504,14 @@ template <> struct ParseMeasurement<BearingRange2D> {
|
|||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
GraphAndValues load2D(const string &filename, SharedNoiseModel model,
|
||||
GraphAndValues load2D(const std::string &filename, SharedNoiseModel model,
|
||||
size_t maxIndex, bool addNoise, bool smart,
|
||||
NoiseFormat noiseFormat,
|
||||
KernelFunctionType kernelFunctionType) {
|
||||
|
||||
// Single pass for poses and landmarks.
|
||||
auto initial = std::make_shared<Values>();
|
||||
Parser<int> insert = [maxIndex, &initial](istream &is, const string &tag) {
|
||||
Parser<int> insert = [maxIndex, &initial](std::istream &is, const std::string &tag) {
|
||||
if (auto indexedPose = parseVertexPose(is, tag)) {
|
||||
if (!maxIndex || indexedPose->first <= maxIndex)
|
||||
initial->insert(indexedPose->first, indexedPose->second);
|
||||
|
|
@ -531,7 +536,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model,
|
|||
|
||||
// Combine in a single parser that adds factors to `graph`, but also inserts
|
||||
// new variables into `initial` when needed.
|
||||
Parser<int> parse = [&](istream &is, const string &tag) {
|
||||
Parser<int> parse = [&](std::istream &is, const std::string &tag) {
|
||||
if (auto f = parseBetweenFactor(is, tag)) {
|
||||
graph->push_back(*f);
|
||||
|
||||
|
|
@ -562,19 +567,20 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model,
|
|||
|
||||
parseLines(filename, parse);
|
||||
|
||||
return make_pair(graph, initial);
|
||||
return {graph, initial};
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, size_t maxIndex,
|
||||
bool addNoise, bool smart, NoiseFormat noiseFormat,
|
||||
GraphAndValues load2D(std::pair<std::string, SharedNoiseModel> dataset,
|
||||
size_t maxIndex, bool addNoise, bool smart,
|
||||
NoiseFormat noiseFormat,
|
||||
KernelFunctionType kernelFunctionType) {
|
||||
return load2D(dataset.first, dataset.second, maxIndex, addNoise, smart,
|
||||
noiseFormat, kernelFunctionType);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GraphAndValues load2D_robust(const string &filename,
|
||||
GraphAndValues load2D_robust(const std::string &filename,
|
||||
const noiseModel::Base::shared_ptr &model,
|
||||
size_t maxIndex) {
|
||||
return load2D(filename, model, maxIndex);
|
||||
|
|
@ -583,9 +589,9 @@ GraphAndValues load2D_robust(const string &filename,
|
|||
/* ************************************************************************* */
|
||||
void save2D(const NonlinearFactorGraph &graph, const Values &config,
|
||||
const noiseModel::Diagonal::shared_ptr model,
|
||||
const string &filename) {
|
||||
const std::string &filename) {
|
||||
|
||||
fstream stream(filename.c_str(), fstream::out);
|
||||
std::fstream stream(filename.c_str(), std::fstream::out);
|
||||
|
||||
// save poses
|
||||
for (const auto &key_pose : config.extract<Pose2>()) {
|
||||
|
|
@ -614,7 +620,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GraphAndValues readG2o(const string &g2oFile, const bool is3D,
|
||||
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D,
|
||||
KernelFunctionType kernelFunctionType) {
|
||||
if (is3D) {
|
||||
return load3D(g2oFile);
|
||||
|
|
@ -630,8 +636,8 @@ GraphAndValues readG2o(const string &g2oFile, const bool is3D,
|
|||
|
||||
/* ************************************************************************* */
|
||||
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
|
||||
const string &filename) {
|
||||
fstream stream(filename.c_str(), fstream::out);
|
||||
const std::string &filename) {
|
||||
std::fstream stream(filename.c_str(), std::fstream::out);
|
||||
|
||||
// Use a lambda here to more easily modify behavior in future.
|
||||
auto index = [](gtsam::Key key) { return Symbol(key).index(); };
|
||||
|
|
@ -676,7 +682,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
|
|||
std::dynamic_pointer_cast<noiseModel::Gaussian>(model);
|
||||
if (!gaussianModel) {
|
||||
model->print("model\n");
|
||||
throw invalid_argument("writeG2o: invalid noise model!");
|
||||
throw std::invalid_argument("writeG2o: invalid noise model!");
|
||||
}
|
||||
Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R();
|
||||
Pose2 pose = factor->measured(); //.inverse();
|
||||
|
|
@ -700,7 +706,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
|
|||
std::dynamic_pointer_cast<noiseModel::Gaussian>(model);
|
||||
if (!gaussianModel) {
|
||||
model->print("model\n");
|
||||
throw invalid_argument("writeG2o: invalid noise model!");
|
||||
throw std::invalid_argument("writeG2o: invalid noise model!");
|
||||
}
|
||||
Matrix6 Info = gaussianModel->R().transpose() * gaussianModel->R();
|
||||
const Pose3 pose3D = factor3D->measured();
|
||||
|
|
@ -731,7 +737,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
|
|||
|
||||
/* ************************************************************************* */
|
||||
// parse quaternion in x,y,z,w order, and normalize to unit length
|
||||
istream &operator>>(istream &is, Quaternion &q) {
|
||||
std::istream &operator>>(std::istream &is, Quaternion &q) {
|
||||
double x, y, z, w;
|
||||
is >> x >> y >> z >> w;
|
||||
const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm;
|
||||
|
|
@ -741,7 +747,7 @@ istream &operator>>(istream &is, Quaternion &q) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// parse Rot3 from roll, pitch, yaw
|
||||
istream &operator>>(istream &is, Rot3 &R) {
|
||||
std::istream &operator>>(std::istream &is, Rot3 &R) {
|
||||
double yaw, pitch, roll;
|
||||
is >> roll >> pitch >> yaw; // notice order !
|
||||
R = Rot3::Ypr(yaw, pitch, roll);
|
||||
|
|
@ -749,20 +755,20 @@ istream &operator>>(istream &is, Rot3 &R) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::optional<pair<size_t, Pose3>> parseVertexPose3(istream &is,
|
||||
const string &tag) {
|
||||
std::optional<std::pair<size_t, Pose3>> parseVertexPose3(std::istream &is,
|
||||
const std::string &tag) {
|
||||
if (tag == "VERTEX3") {
|
||||
size_t id;
|
||||
double x, y, z;
|
||||
Rot3 R;
|
||||
is >> id >> x >> y >> z >> R;
|
||||
return make_pair(id, Pose3(R, {x, y, z}));
|
||||
return std::make_pair(id, Pose3(R, {x, y, z}));
|
||||
} else if (tag == "VERTEX_SE3:QUAT") {
|
||||
size_t id;
|
||||
double x, y, z;
|
||||
Quaternion q;
|
||||
is >> id >> x >> y >> z >> q;
|
||||
return make_pair(id, Pose3(q, {x, y, z}));
|
||||
return std::make_pair(id, Pose3(q, {x, y, z}));
|
||||
} else
|
||||
return std::nullopt;
|
||||
}
|
||||
|
|
@ -774,13 +780,13 @@ GTSAM_EXPORT std::map<size_t, Pose3> parseVariables<Pose3>(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::optional<pair<size_t, Point3>> parseVertexPoint3(istream &is,
|
||||
const string &tag) {
|
||||
std::optional<std::pair<size_t, Point3>> parseVertexPoint3(std::istream &is,
|
||||
const std::string &tag) {
|
||||
if (tag == "VERTEX_TRACKXYZ") {
|
||||
size_t id;
|
||||
double x, y, z;
|
||||
is >> id >> x >> y >> z;
|
||||
return make_pair(id, Point3(x, y, z));
|
||||
return std::make_pair(id, Point3(x, y, z));
|
||||
} else
|
||||
return std::nullopt;
|
||||
}
|
||||
|
|
@ -793,7 +799,7 @@ GTSAM_EXPORT std::map<size_t, Point3> parseVariables<Point3>(
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Parse a symmetric covariance matrix (onlyupper-triangular part is stored)
|
||||
istream &operator>>(istream &is, Matrix6 &m) {
|
||||
std::istream &operator>>(std::istream &is, Matrix6 &m) {
|
||||
for (size_t i = 0; i < 6; i++)
|
||||
for (size_t j = i; j < 6; j++) {
|
||||
is >> m(i, j);
|
||||
|
|
@ -810,8 +816,8 @@ template <> struct ParseMeasurement<Pose3> {
|
|||
size_t maxIndex;
|
||||
|
||||
// The actual parser
|
||||
std::optional<BinaryMeasurement<Pose3>> operator()(istream &is,
|
||||
const string &tag) {
|
||||
std::optional<BinaryMeasurement<Pose3>> operator()(std::istream &is,
|
||||
const std::string &tag) {
|
||||
if (tag != "EDGE3" && tag != "EDGE_SE3:QUAT")
|
||||
return std::nullopt;
|
||||
|
||||
|
|
@ -915,7 +921,7 @@ parseFactors<Pose3>(const std::string &filename,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GraphAndValues load3D(const string &filename) {
|
||||
GraphAndValues load3D(const std::string &filename) {
|
||||
auto graph = std::make_shared<NonlinearFactorGraph>();
|
||||
auto initial = std::make_shared<Values>();
|
||||
|
||||
|
|
@ -924,7 +930,7 @@ GraphAndValues load3D(const string &filename) {
|
|||
|
||||
// Single pass for variables and factors. Unlike 2D version, does *not* insert
|
||||
// variables into `initial` if referenced but not present.
|
||||
Parser<int> parse = [&](istream &is, const string &tag) {
|
||||
Parser<int> parse = [&](std::istream &is, const std::string &tag) {
|
||||
if (auto indexedPose = parseVertexPose3(is, tag)) {
|
||||
initial->insert(indexedPose->first, indexedPose->second);
|
||||
} else if (auto indexedLandmark = parseVertexPoint3(is, tag)) {
|
||||
|
|
@ -936,7 +942,7 @@ GraphAndValues load3D(const string &filename) {
|
|||
};
|
||||
parseLines(filename, parse);
|
||||
|
||||
return make_pair(graph, initial);
|
||||
return {graph, initial};
|
||||
}
|
||||
|
||||
// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
|
||||
|
|
|
|||
Loading…
Reference in New Issue