From f08f8afdd0ef0885433386c7fba88d42c354b676 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Feb 2023 13:35:40 -0800 Subject: [PATCH] Do not use std... --- gtsam/slam/dataset.cpp | 130 +++++++++++++++++++++-------------------- 1 file changed, 68 insertions(+), 62 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 6ef0692c1..bbb4c2343 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -47,30 +47,35 @@ #include #include #include +#include + #if defined(__GNUC__) && (__GNUC__ == 7) #include #else #include #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 rootsToSearch; + std::vector 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 namesToSearch; + std::vector 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 using Parser = - std::function(istream &is, const string &tag)>; + std::function(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 -static void parseLines(const string &filename, Parser parse) { - ifstream is(filename.c_str()); +static void parseLines(const std::string &filename, Parser 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 parse) { /* ************************************************************************* */ // Parse types T into a size_t-indexed map template -map parseToMap(const string &filename, Parser> parse, +std::map parseToMap(const std::string &filename, Parser> parse, size_t maxIndex) { - map result; - Parser> emplace = [&](istream &is, const string &tag) { + std::map result; + Parser> 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 parseToMap(const string &filename, Parser> parse, /* ************************************************************************* */ // Parse a file and push results on a vector template -static vector parseToVector(const string &filename, Parser parse) { - vector result; - Parser add = [&result, parse](istream &is, const string &tag) { +static std::vector parseToVector(const std::string &filename, Parser parse) { + std::vector result; + Parser 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 parseToVector(const string &filename, Parser parse) { } /* ************************************************************************* */ -std::optional parseVertexPose(istream &is, const string &tag) { +std::optional 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 parseVariables( } /* ************************************************************************* */ -std::optional parseVertexLandmark(istream &is, - const string &tag) { +std::optional 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 parseEdge(istream &is, const string &tag) { +std::optional parseEdge(std::istream &is, const std::string &tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || (tag == "ODOMETRY")) { @@ -318,7 +323,7 @@ template struct ParseFactor : ParseMeasurement { // We parse a measurement then convert typename std::optional::shared_ptr> - operator()(istream &is, const string &tag) { + operator()(std::istream &is, const std::string &tag) { if (auto m = ParseMeasurement::operator()(is, tag)) return std::make_shared>( m->key1(), m->key2(), m->measured(), m->noiseModel()); @@ -343,8 +348,8 @@ template <> struct ParseMeasurement { SharedNoiseModel model; // The actual parser - std::optional> operator()(istream &is, - const string &tag) { + std::optional> 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 { // 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 { std::shared_ptr createSampler(const SharedNoiseModel &model) { auto noise = std::dynamic_pointer_cast(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(new Sampler(noise)); } @@ -449,7 +454,7 @@ template <> struct ParseMeasurement { // The actual parser std::optional> - 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 { }; /* ************************************************************************* */ -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(); - Parser insert = [maxIndex, &initial](istream &is, const string &tag) { + Parser 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 parse = [&](istream &is, const string &tag) { + Parser 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 dataset, size_t maxIndex, - bool addNoise, bool smart, NoiseFormat noiseFormat, +GraphAndValues load2D(std::pair 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()) { @@ -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(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(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> parseVertexPose3(istream &is, - const string &tag) { +std::optional> 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 parseVariables( } /* ************************************************************************* */ -std::optional> parseVertexPoint3(istream &is, - const string &tag) { +std::optional> 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 parseVariables( /* ************************************************************************* */ // 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 { size_t maxIndex; // The actual parser - std::optional> operator()(istream &is, - const string &tag) { + std::optional> operator()(std::istream &is, + const std::string &tag) { if (tag != "EDGE3" && tag != "EDGE_SE3:QUAT") return std::nullopt; @@ -915,7 +921,7 @@ parseFactors(const std::string &filename, } /* ************************************************************************* */ -GraphAndValues load3D(const string &filename) { +GraphAndValues load3D(const std::string &filename) { auto graph = std::make_shared(); auto initial = std::make_shared(); @@ -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 parse = [&](istream &is, const string &tag) { + Parser 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 and parseFactors