12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Do not use std...

release/4.3a0
Frank Dellaert 2023-02-04 13:35:40 -08:00
parent 22e14228e2
commit f08f8afdd0
1 changed files with 68 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
gtsam/slam/dataset.cpp
View File

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