From a978c15d8e17eeac95b334300e9b4fdff6e4110d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 12 Aug 2020 19:41:56 -0400 Subject: [PATCH 01/10] Templated some methods internally --- gtsam/slam/dataset.cpp | 360 ++++++++++++++++++------------- gtsam/slam/dataset.h | 45 ++-- gtsam/slam/tests/testDataset.cpp | 34 ++- 3 files changed, 267 insertions(+), 172 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 3a2d04127..4f56d3afe 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -105,10 +105,10 @@ string createRewrittenFileName(const string& name) { } /* ************************************************************************* */ -GraphAndValues load2D(pair dataset, int maxID, +GraphAndValues load2D(pair dataset, Key maxNr, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - return load2D(dataset.first, dataset.second, maxID, addNoise, smart, + return load2D(dataset.first, dataset.second, maxNr, addNoise, smart, noiseFormat, kernelFunctionType); } @@ -226,6 +226,54 @@ boost::optional parseVertexLandmark(istream& is, const string& } } +/* ************************************************************************* */ +// Parse types T into a Key-indexed map +template +static map parseIntoMap(const string &filename, Key maxNr = 0) { + ifstream is(filename.c_str()); + if (!is) + throw invalid_argument("parse: can not find file " + filename); + + map result; + string tag; + while (!is.eof()) { + boost::optional> indexed_t; + is >> indexed_t; + if (indexed_t) { + // optional filter + if (maxNr && indexed_t->first >= maxNr) + continue; + result.insert(*indexed_t); + } + is.ignore(LINESIZE, '\n'); + } + return result; +} + +/* ************************************************************************* */ +istream &operator>>(istream &is, boost::optional &indexed) { + string tag; + is >> tag; + indexed = parseVertexPose(is, tag); + return is; +} + +map parse2DPoses(const string &filename, Key maxNr) { + return parseIntoMap(filename, maxNr); +} + +/* ************************************************************************* */ +istream &operator>>(istream &is, boost::optional &indexed) { + string tag; + is >> tag; + indexed = parseVertexLandmark(is, tag); + return is; +} + +map parse2DLandmarks(const string &filename, Key maxNr) { + return parseIntoMap(filename, maxNr); +} + /* ************************************************************************* */ boost::optional parseEdge(istream& is, const string& tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") @@ -241,49 +289,27 @@ boost::optional parseEdge(istream& is, const string& tag) { } /* ************************************************************************* */ -GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, - bool addNoise, bool smart, NoiseFormat noiseFormat, - KernelFunctionType kernelFunctionType) { +GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, + bool addNoise, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + + Values::shared_ptr initial(new Values); + + const auto poses = parse2DPoses(filename, maxNr); + for (const auto& key_pose : poses) { + initial->insert(key_pose.first, key_pose.second); + } + const auto landmarks = parse2DLandmarks(filename, maxNr); + for (const auto& key_landmark : landmarks) { + initial->insert(key_landmark.first, key_landmark.second); + } ifstream is(filename.c_str()); if (!is) throw invalid_argument("load2D: can not find file " + filename); - Values::shared_ptr initial(new Values); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); - string tag; - - // load the poses and landmarks - while (!is.eof()) { - if (!(is >> tag)) - break; - - const auto indexed_pose = parseVertexPose(is, tag); - if (indexed_pose) { - Key id = indexed_pose->first; - - // optional filter - if (maxID && id >= maxID) - continue; - - initial->insert(id, indexed_pose->second); - } - const auto indexed_landmark = parseVertexLandmark(is, tag); - if (indexed_landmark) { - Key id = indexed_landmark->first; - - // optional filter - if (maxID && id >= maxID) - continue; - - initial->insert(id, indexed_landmark->second); - } - is.ignore(LINESIZE, '\n'); - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - // If asked, create a sampler with random number generator std::unique_ptr sampler; if (addNoise) { @@ -302,6 +328,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, bool haveLandmark = false; const bool useModelInFile = !model; while (!is.eof()) { + string tag; if (!(is >> tag)) break; @@ -315,7 +342,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, kernelFunctionType); // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) + if (maxNr && (id1 >= maxNr || id2 >= maxNr)) continue; if (useModelInFile) @@ -375,7 +402,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (tag == "LANDMARK" || tag == "BR") { // optional filter - if (maxID && id1 >= maxID) + if (maxNr && id1 >= maxNr) continue; // Create noise model @@ -404,8 +431,8 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, /* ************************************************************************* */ GraphAndValues load2D_robust(const string& filename, - noiseModel::Base::shared_ptr& model, int maxID) { - return load2D(filename, model, maxID); + noiseModel::Base::shared_ptr& model, Key maxNr) { + return load2D(filename, model, maxNr); } /* ************************************************************************* */ @@ -448,10 +475,10 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D, return load3D(g2oFile); } else { // just call load2D - int maxID = 0; + Key maxNr = 0; bool addNoise = false; bool smart = true; - return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + return load2D(g2oFile, SharedNoiseModel(), maxNr, addNoise, smart, NoiseFormatG2O, kernelFunctionType); } } @@ -516,8 +543,8 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, Pose2 pose = factor->measured(); //.inverse(); stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " << pose.x() << " " << pose.y() << " " << pose.theta(); - for (int i = 0; i < 3; i++){ - for (int j = i; j < 3; j++){ + for (size_t i = 0; i < 3; i++){ + for (size_t j = i; j < 3; j++){ stream << " " << Info(i, j); } } @@ -545,13 +572,13 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, << " " << q.y() << " " << q.z() << " " << q.w(); Matrix InfoG2o = I_6x6; - InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation - InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation - InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal - InfoG2o.block(3,0,3,3) = Info.block(3,0,3,3); // off diagonal + InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation + InfoG2o.block<3, 3>(3, 3) = Info.block<3, 3>(0, 0); // cov rotation + InfoG2o.block<3, 3>(0, 3) = Info.block<3, 3>(0, 3); // off diagonal + InfoG2o.block<3, 3>(3, 0) = Info.block<3, 3>(3, 0); // off diagonal - for (int i = 0; i < 6; i++){ - for (int j = i; j < 6; j++){ + for (size_t i = 0; i < 6; i++){ + for (size_t j = i; j < 6; j++){ stream << " " << InfoG2o(i, j); } } @@ -562,126 +589,157 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } /* ************************************************************************* */ -static Rot3 NormalizedRot3(double w, double x, double y, double z) { +// parse quaternion in x,y,z,w order, and normalize to unit length +istream &operator>>(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; - return Rot3::Quaternion(f * w, f * x, f * y, f * z); + q = Quaternion(f * w, f * x, f * y, f * z); + return is; } /* ************************************************************************* */ -std::map parse3DPoses(const string& filename) { +// parse Rot3 from roll, pitch, yaw +istream &operator>>(istream &is, Rot3 &R) { + double yaw, pitch, roll; + is >> roll >> pitch >> yaw; // notice order ! + R = Rot3::Ypr(yaw, pitch, roll); + return is; +} + +/* ************************************************************************* */ +istream &operator>>(istream &is, boost::optional> &indexed) { + string tag; + is >> tag; + if (tag == "VERTEX3") { + Key id; + double x, y, z; + Rot3 R; + is >> id >> x >> y >> z >> R; + indexed.reset({id, Pose3(R, {x, y, z})}); + } else if (tag == "VERTEX_SE3:QUAT") { + Key id; + double x, y, z; + Quaternion q; + is >> id >> x >> y >> z >> q; + indexed.reset({id, Pose3(q, {x, y, z})}); + } + return is; +} + +map parse3DPoses(const string &filename, Key maxNr) { + return parseIntoMap(filename, maxNr); +} + +/* ************************************************************************* */ +istream &operator>>(istream &is, boost::optional> &indexed) { + string tag; + is >> tag; + if (tag == "VERTEX_TRACKXYZ") { + Key id; + double x, y, z; + is >> id >> x >> y >> z; + indexed.reset({id, Point3(x, y, z)}); + } + return is; +} + +map parse3DLandmarks(const string &filename, Key maxNr) { + return parseIntoMap(filename, maxNr); +} + +/* ************************************************************************* */ +// Parse BetweenFactor shared pointers into a vector +template +static vector>> +parseIntoVector(const string &filename, Key maxNr = 0) { ifstream is(filename.c_str()); if (!is) - throw invalid_argument("parse3DPoses: can not find file " + filename); + throw invalid_argument("parse: can not find file " + filename); - std::map poses; + vector>> result; + string tag; while (!is.eof()) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; - - if (tag == "VERTEX3") { - Key id; - double x, y, z, roll, pitch, yaw; - ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); - } - if (tag == "VERTEX_SE3:QUAT") { - Key id; - double x, y, z, qx, qy, qz, qw; - ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - poses.emplace(id, Pose3(NormalizedRot3(qw, qx, qy, qz), {x, y, z})); + boost::shared_ptr> shared; + is >> shared; + if (shared) { + // optional filter + if (maxNr && (shared->key1() >= maxNr || shared->key1() >= maxNr)) + continue; + result.push_back(shared); } + is.ignore(LINESIZE, '\n'); } - return poses; + return result; } /* ************************************************************************* */ -std::map parse3DLandmarks(const string& filename) { - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("parse3DLandmarks: can not find file " + filename); - - std::map landmarks; - while (!is.eof()) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; - - if (tag == "VERTEX_TRACKXYZ") { - Key id; - double x, y, z; - ls >> id >> x >> y >> z; - landmarks.emplace(id, Point3(x, y, z)); - } +// Parse a symmetric covariance matrix (onlyupper-triangular part is stored) +istream &operator>>(istream &is, Matrix6 &m) { + for (size_t i = 0; i < 6; i++) + for (size_t j = i; j < 6; j++){ + is >> m(i, j); + m(j,i) = m(i,j); } - return landmarks; + return is; } /* ************************************************************************* */ -BetweenFactorPose3s parse3DFactors( - const string& filename, - const noiseModel::Diagonal::shared_ptr& corruptingNoise) { - ifstream is(filename.c_str()); - if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); +istream &operator>>(istream &is, + boost::shared_ptr> &shared) { + string tag; + is >> tag; + + Matrix6 m; + if (tag == "EDGE3") { + Key id1, id2; + double x, y, z; + Rot3 R; + is >> id1 >> id2 >> x >> y >> z >> R; + Pose3 pose(R, {x, y, z}); + + is >> m; + SharedNoiseModel model = noiseModel::Gaussian::Information(m); + shared.reset(new BetweenFactor(id1, id2, pose, model)); + } + if (tag == "EDGE_SE3:QUAT") { + Key id1, id2; + double x, y, z; + Quaternion q; + is >> id1 >> id2 >> x >> y >> z >> q; + Pose3 pose(q, {x, y, z}); + + // EDGE_SE3:QUAT stores covariance in t,R order, unlike GTSAM: + is >> m; + Matrix6 mgtsam; + mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation + mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal + SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); + + shared.reset(new BetweenFactor(id1, id2, pose, model)); + } + return is; +} + +/* ************************************************************************* */ +BetweenFactorPose3s +parse3DFactors(const string &filename, + const noiseModel::Diagonal::shared_ptr &corruptingNoise, + Key maxNr) { + auto factors = parseIntoVector(filename, maxNr); - boost::optional sampler; if (corruptingNoise) { - sampler = Sampler(corruptingNoise); - } - - std::vector::shared_ptr> factors; - while (!is.eof()) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; - - if (tag == "EDGE3") { - Key id1, id2; - double x, y, z, roll, pitch, yaw; - ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Matrix m(6, 6); - for (size_t i = 0; i < 6; i++) - for (size_t j = i; j < 6; j++) ls >> m(i, j); - SharedNoiseModel model = noiseModel::Gaussian::Information(m); - factors.emplace_back(new BetweenFactor( - id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); - } - if (tag == "EDGE_SE3:QUAT") { - Key id1, id2; - double x, y, z, qx, qy, qz, qw; - ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Matrix m(6, 6); - for (size_t i = 0; i < 6; i++) { - for (size_t j = i; j < 6; j++) { - double mij; - ls >> mij; - m(i, j) = mij; - m(j, i) = mij; - } - } - Matrix mgtsam(6, 6); - - mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation - mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation - mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal - mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal - - SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - auto R12 = NormalizedRot3(qw, qx, qy, qz); - if (sampler) { - R12 = R12.retract(sampler->sample()); - } - - factors.emplace_back(new BetweenFactor( - id1, id2, Pose3(R12, {x, y, z}), model)); + Sampler sampler(corruptingNoise); + for (auto factor : factors) { + auto pose = factor->measured(); + factor.reset(new BetweenFactor(factor->key1(), factor->key2(), + pose.retract(sampler.sample()), + factor->noiseModel())); } } + return factors; } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 30fa913ba..caef96cf4 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -23,10 +23,8 @@ #include #include #include -#include -#include +#include #include -#include #include #include #include @@ -114,18 +112,35 @@ GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& GTSAM_EXPORT boost::optional parseEdge(std::istream& is, const std::string& tag); +using BetweenFactorPose2s = + std::vector::shared_ptr>; + +/// Parse edges in 2D g2o graph file into a set of BetweenFactors. +GTSAM_EXPORT BetweenFactorPose2s parse2DFactors( + const std::string &filename, + const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr); + +/// Parse vertices in 2D g2o graph file into a map of Pose2s. +GTSAM_EXPORT std::map parse2DPoses(const std::string &filename, + Key maxNr = 0); + +/// Parse landmarks in 2D g2o graph file into a map of Point2s. +GTSAM_EXPORT std::map parse2DLandmarks(const string &filename, + Key maxNr = 0); + /// Return type for load functions -typedef std::pair GraphAndValues; +using GraphAndValues = + std::pair; /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] - * @param maxID if non-zero cut out vertices >= maxID + * @param maxNr if non-zero cut out vertices >= maxNr * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ GTSAM_EXPORT GraphAndValues load2D( - std::pair dataset, int maxID = 0, + std::pair dataset, Key maxNr = 0, bool addNoise = false, bool smart = true, // NoiseFormat noiseFormat = NoiseFormatAUTO, @@ -135,7 +150,7 @@ GTSAM_EXPORT GraphAndValues load2D( * Load TORO/G2O style graph files * @param filename * @param model optional noise model to use instead of one specified by file - * @param maxID if non-zero cut out vertices >= maxID + * @param maxNr if non-zero cut out vertices >= maxNr * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model * @param noiseFormat how noise parameters are stored @@ -143,13 +158,13 @@ GTSAM_EXPORT GraphAndValues load2D( * @return graph and initial values */ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, - SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise = + SharedNoiseModel model = SharedNoiseModel(), Key maxNr = 0, bool addNoise = false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, - noiseModel::Base::shared_ptr& model, int maxID = 0); + noiseModel::Base::shared_ptr& model, Key maxNr = 0); /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, @@ -179,14 +194,18 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, /// Parse edges in 3D TORO graph file into a set of BetweenFactors. using BetweenFactorPose3s = std::vector::shared_ptr>; -GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, - const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr); +GTSAM_EXPORT BetweenFactorPose3s parse3DFactors( + const std::string &filename, + const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr, + Key maxNr = 0); /// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s. -GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); +GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, + Key maxNr = 0); /// Parse landmarks in 3D g2o graph file into a map of Point3s. -GTSAM_EXPORT std::map parse3DLandmarks(const string& filename); +GTSAM_EXPORT std::map parse3DLandmarks(const std::string &filename, + Key maxNr = 0); /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index b6b1776d2..6ee44cfd6 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -89,20 +89,38 @@ TEST( dataSet, parseEdge) } /* ************************************************************************* */ -TEST( dataSet, load2D) -{ +TEST(dataSet, load2D) { ///< The structure where we will save the SfM data const string filename = findExampleDataFile("w100.graph"); NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; boost::tie(graph, initial) = load2D(filename); - EXPECT_LONGS_EQUAL(300,graph->size()); - EXPECT_LONGS_EQUAL(100,initial->size()); - noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); - BetweenFactor expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model); - BetweenFactor::shared_ptr actual = boost::dynamic_pointer_cast< - BetweenFactor >(graph->at(0)); + EXPECT_LONGS_EQUAL(300, graph->size()); + EXPECT_LONGS_EQUAL(100, initial->size()); + auto model = noiseModel::Unit::Create(3); + BetweenFactor expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381), + model); + BetweenFactor::shared_ptr actual = + boost::dynamic_pointer_cast>(graph->at(0)); EXPECT(assert_equal(expected, *actual)); + + // // Check factor parsing + // const auto actualFactors = parse2DFactors(filename); + // for (size_t i : {0, 1, 2, 3, 4, 5}) { + // EXPECT(assert_equal( + // *boost::dynamic_pointer_cast>(graph->at(i)), + // *actualFactors[i], 1e-5)); + // } + + // Check pose parsing + const auto actualPoses = parse2DPoses(filename); + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(initial->at(j), actualPoses.at(j), 1e-5)); + } + + // Check landmark parsing + const auto actualLandmarks = parse2DLandmarks(filename); + EXPECT_LONGS_EQUAL(0, actualLandmarks.size()); } /* ************************************************************************* */ From d67afa8a3d141cc1b913c6fb88ddac74b1c55c1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 12 Aug 2020 23:24:52 -0400 Subject: [PATCH 02/10] Very generic parseToVector --- gtsam/slam/dataset.cpp | 576 +++++++++++++++++++------------ gtsam/slam/dataset.h | 24 +- gtsam/slam/tests/testDataset.cpp | 14 +- 3 files changed, 370 insertions(+), 244 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4f56d3afe..29162dafb 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -21,21 +21,25 @@ #include #include #include + #include #include #include + +#include +#include + +#include + #include #include -#include -#include -#include -#include + #include #include #include -#include #include #include +#include #include #include @@ -56,7 +60,7 @@ using namespace gtsam::symbol_shorthand; namespace gtsam { /* ************************************************************************* */ -string findExampleDataFile(const string& name) { +string findExampleDataFile(const string &name) { // Search source tree and installed location vector rootsToSearch; @@ -73,88 +77,88 @@ string findExampleDataFile(const string& name) { namesToSearch.push_back(name + ".xml"); // Find first name that exists - for(const fs::path root: rootsToSearch) { - for(const fs::path name: namesToSearch) { + for (const fs::path root : rootsToSearch) { + for (const fs::path name : namesToSearch) { if (fs::is_regular_file(root / name)) return (root / name).string(); } } // If we did not return already, then we did not find the file - throw 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" + name + ", " + name - + ".graph, or " + name + ".txt"); + throw 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" + + name + ", " + name + ".graph, or " + name + ".txt"); } /* ************************************************************************* */ -string createRewrittenFileName(const string& name) { +string createRewrittenFileName(const string &name) { // Search source tree and installed location if (!exists(fs::path(name))) { throw invalid_argument( - "gtsam::createRewrittenFileName could not find a matching file in\n" - + name); + "gtsam::createRewrittenFileName could not find a matching file in\n" + + name); } fs::path p(name); - fs::path newpath = fs::path(p.parent_path().string()) - / fs::path(p.stem().string() + "-rewritten.txt"); + fs::path newpath = fs::path(p.parent_path().string()) / + fs::path(p.stem().string() + "-rewritten.txt"); return newpath.string(); } /* ************************************************************************* */ GraphAndValues load2D(pair dataset, Key maxNr, - bool addNoise, bool smart, NoiseFormat noiseFormat, - KernelFunctionType kernelFunctionType) { + bool addNoise, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { return load2D(dataset.first, dataset.second, maxNr, addNoise, smart, - noiseFormat, kernelFunctionType); + noiseFormat, kernelFunctionType); } /* ************************************************************************* */ -// Read noise parameters and interpret them according to flags -static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, - NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - double v1, v2, v3, v4, v5, v6; - is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; - +// Interpret noise parameters according to flags +static SharedNoiseModel +createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { if (noiseFormat == NoiseFormatAUTO) { // Try to guess covariance matrix layout - if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 - && v6 == 0.0) { - // NoiseFormatGRAPH + if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && // + v(3) != 0.0 && v(4) == 0.0 && v(5) == 0.0) { noiseFormat = NoiseFormatGRAPH; - } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 - && v6 != 0.0) { - // NoiseFormatCOV + } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && // + v(3) != 0.0 && v(4) == 0.0 && v(5) != 0.0) { noiseFormat = NoiseFormatCOV; } else { throw std::invalid_argument( - "load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); + "load2D: unrecognized covariance matrix format in dataset file. " + "Please specify the noise format."); } } // Read matrix and check that diagonal entries are non-zero - Matrix M(3, 3); + Matrix3 M; switch (noiseFormat) { case NoiseFormatG2O: case NoiseFormatCOV: - // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] - if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0) + // i.e., [v(0) v(1) v(2); + // 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( "load2D::readNoiseModel looks like this is not G2O matrix order"); - M << v1, v2, v3, v2, v4, v5, v3, v5, v6; + M << v(0), v(1), v(2), v(1), v(3), v(4), v(2), v(4), v(5); break; case NoiseFormatTORO: case NoiseFormatGRAPH: // http://www.openslam.org/toro.html // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr - // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] - if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0) + // i.e., [v(0) v(1) v(4); + // 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( "load2D::readNoiseModel looks like this is not TORO matrix order"); - M << v1, v2, v5, v2, v3, v6, v5, v6, v4; + 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"); @@ -195,15 +199,18 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, } } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /* ************************************************************************* */ -boost::optional parseVertex(istream& is, const string& tag) { - return parseVertexPose(is, tag); +// Read noise parameters and interpret them according to flags +static SharedNoiseModel readNoiseModel(istream &is, bool smart, + NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + Vector6 v; + is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); + return createNoiseModel(v, smart, noiseFormat, kernelFunctionType); } -#endif /* ************************************************************************* */ -boost::optional parseVertexPose(istream& is, const string& tag) { +boost::optional parseVertexPose(istream &is, const string &tag) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { Key id; double x, y, yaw; @@ -215,7 +222,8 @@ boost::optional parseVertexPose(istream& is, const string& tag) { } /* ************************************************************************* */ -boost::optional parseVertexLandmark(istream& is, const string& tag) { +boost::optional parseVertexLandmark(istream &is, + const string &tag) { if (tag == "VERTEX_XY") { Key id; double x, y; @@ -275,9 +283,37 @@ map parse2DLandmarks(const string &filename, Key maxNr) { } /* ************************************************************************* */ -boost::optional parseEdge(istream& is, const string& tag) { - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") - || (tag == "ODOMETRY")) { +// Parse a file by first parsing the `Parsed` type and then processing it with +// the supplied `process` function. Result is put in a vector evaluates to true. +// Requires: a stream >> operator for boost::optional +template +static vector +parseToVector(const string &filename, + const std::function process) { + ifstream is(filename.c_str()); + if (!is) + throw invalid_argument("parse: can not find file " + filename); + + vector result; + string tag; + while (!is.eof()) { + boost::optional parsed; + is >> parsed; + if (parsed) { + Output factor = process(*parsed); // can return nullptr + if (factor) { + result.push_back(factor); + } + } + is.ignore(LINESIZE, '\n'); + } + return result; +} + +/* ************************************************************************* */ +boost::optional parseEdge(istream &is, const string &tag) { + if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || + (tag == "ODOMETRY")) { Key id1, id2; double x, y, yaw; @@ -288,6 +324,86 @@ boost::optional parseEdge(istream& is, const string& tag) { } } +/* ************************************************************************* */ +struct Measurement2 { + Key id1, id2; + Pose2 pose; + Vector6 v; // 6 noise model parameters for edge +}; + +istream &operator>>(istream &is, boost::optional &edge) { + string tag; + is >> tag; + if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || + (tag == "ODOMETRY")) { + Key id1, id2; + double x, y, yaw; + Vector6 v; + is >> id1 >> id2 >> x >> y >> yaw >> // + v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); + edge.reset({id1, id2, Pose2(x, y, yaw), v}); + } + return is; +} + +/* ************************************************************************* */ +// Create a sampler +boost::shared_ptr createSampler(const SharedNoiseModel &model) { + auto noise = boost::dynamic_pointer_cast(model); + if (!noise) + throw invalid_argument("gtsam::load: invalid noise model for adding noise" + "(current version assumes diagonal noise model)!"); + return boost::shared_ptr(new Sampler(noise)); +} + +/* ************************************************************************* */ +// Small functor to process the edge into a BetweenFactor::shared_ptr +struct ProcessPose2 { + // The arguments + Key maxNr = 0; + SharedNoiseModel model; + boost::shared_ptr sampler; + + // Arguments for parsing noise model + bool smart = true; + NoiseFormat noiseFormat = NoiseFormatAUTO; + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE; + + // The actual function + BetweenFactor::shared_ptr operator()(const Measurement2 &edge) { + // optional filter + if (maxNr && (edge.id1 >= maxNr || edge.id2 >= maxNr)) + return nullptr; + + // Get pose and optionally add noise + Pose2 T12 = edge.pose; + if (sampler) + T12 = T12.retract(sampler->sample()); + + // Create factor + // If model is nullptr we use the model from the file + return boost::make_shared>( + edge.id1, edge.id2, T12, + model + ? model + : createNoiseModel(edge.v, smart, noiseFormat, kernelFunctionType)); + } +}; + +/* ************************************************************************* */ +BetweenFactorPose2s +parse2DFactors(const string &filename, + const noiseModel::Diagonal::shared_ptr &corruptingNoise, + Key maxNr) { + ProcessPose2 process; + process.maxNr = maxNr; + if (corruptingNoise) { + process.sampler = createSampler(corruptingNoise); + } + return parseToVector::shared_ptr>(filename, + process); +} + /* ************************************************************************* */ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, bool addNoise, bool smart, NoiseFormat noiseFormat, @@ -296,11 +412,11 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, Values::shared_ptr initial(new Values); const auto poses = parse2DPoses(filename, maxNr); - for (const auto& key_pose : poses) { + for (const auto &key_pose : poses) { initial->insert(key_pose.first, key_pose.second); } const auto landmarks = parse2DLandmarks(filename, maxNr); - for (const auto& key_landmark : landmarks) { + for (const auto &key_landmark : landmarks) { initial->insert(key_landmark.first, key_landmark.second); } @@ -311,7 +427,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); // If asked, create a sampler with random number generator - std::unique_ptr sampler; + boost::shared_ptr sampler; if (addNoise) { noiseModel::Diagonal::shared_ptr noise; if (model) @@ -319,7 +435,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, if (!noise) throw invalid_argument( "gtsam::load2D: invalid noise model for adding noise" - "(current version assumes diagonal noise model)!"); + "(current version assumes diagonal noise model)!"); sampler.reset(new Sampler(noise)); } @@ -335,11 +451,11 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, auto between_pose = parseEdge(is, tag); if (between_pose) { std::tie(id1, id2) = between_pose->first; - Pose2& l1Xl2 = between_pose->second; + Pose2 l1Xl2 = between_pose->second; // read noise model - SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat, - kernelFunctionType); + SharedNoiseModel modelInFile = + readNoiseModel(is, smart, noiseFormat, kernelFunctionType); // optional filter if (maxNr && (id1 >= maxNr || id2 >= maxNr)) @@ -369,7 +485,8 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; } - // A landmark measurement, TODO Frank says: don't know why is converted to bearing-range + // A landmark measurement, TODO Frank says: don't know why is converted to + // bearing-range if (tag == "LANDMARK") { double lmx, lmy; double v1, v2, v3; @@ -380,8 +497,9 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, bearing = atan2(lmy, lmx); range = sqrt(lmx * lmx + lmy * lmy); - // In our experience, the x-y covariance on landmark sightings is not very good, so assume - // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. + // In our experience, the x-y covariance on landmark sightings is not + // very good, so assume it describes the uncertainty at a range of 10m, + // and convert that to bearing/range uncertainty. if (std::abs(v1 - v3) < 1e-4) { bearing_std = sqrt(v1 / 10.0); range_std = sqrt(v1); @@ -389,10 +507,11 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, bearing_std = 1; range_std = 1; if (!haveLandmark) { - cout - << "Warning: load2D is a very simple dataset loader and is ignoring the\n" - "non-uniform covariance on LANDMARK measurements in this file." - << endl; + cout << "Warning: load2D is a very simple dataset loader and is " + "ignoring the\n" + "non-uniform covariance on LANDMARK measurements in this " + "file." + << endl; haveLandmark = true; } } @@ -407,11 +526,12 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, // Create noise model noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std).finished()); + noiseModel::Diagonal::Sigmas( + (Vector(2) << bearing_std, range_std).finished()); // Add to graph *graph += BearingRangeFactor(id1, L(id2), bearing, range, - measurementNoise); + measurementNoise); // Insert poses or points if they do not exist yet if (!initial->exists(id1)) @@ -430,46 +550,46 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, } /* ************************************************************************* */ -GraphAndValues load2D_robust(const string& filename, - noiseModel::Base::shared_ptr& model, Key maxNr) { +GraphAndValues load2D_robust(const string &filename, + noiseModel::Base::shared_ptr &model, Key maxNr) { return load2D(filename, model, maxNr); } /* ************************************************************************* */ -void save2D(const NonlinearFactorGraph& graph, const Values& config, - const noiseModel::Diagonal::shared_ptr model, const string& filename) { +void save2D(const NonlinearFactorGraph &graph, const Values &config, + const noiseModel::Diagonal::shared_ptr model, + const string &filename) { fstream stream(filename.c_str(), fstream::out); // save poses - - for(const Values::ConstKeyValuePair& key_value: config) { - const Pose2& pose = key_value.value.cast(); + for (const Values::ConstKeyValuePair &key_value : config) { + const Pose2 &pose = key_value.value.cast(); stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() - << " " << pose.theta() << endl; + << " " << pose.theta() << endl; } // save edges - Matrix R = model->R(); - Matrix RR = trans(R) * R; //prod(trans(R),R); - for(boost::shared_ptr factor_: graph) { - boost::shared_ptr > factor = - boost::dynamic_pointer_cast >(factor_); + // TODO(frank): why don't we use model in factor? + Matrix3 R = model->R(); + Matrix3 RR = R.transpose() * R; + for (auto f : graph) { + auto factor = boost::dynamic_pointer_cast>(f); if (!factor) continue; - Pose2 pose = factor->measured().inverse(); + const Pose2 pose = factor->measured().inverse(); stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " - << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) - << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " - << RR(0, 2) << " " << RR(1, 2) << endl; + << pose.x() << " " << pose.y() << " " << pose.theta() << " " + << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) + << " " << RR(0, 2) << " " << RR(1, 2) << endl; } stream.close(); } /* ************************************************************************* */ -GraphAndValues readG2o(const string& g2oFile, const bool is3D, +GraphAndValues readG2o(const string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType) { if (is3D) { return load3D(g2oFile); @@ -484,86 +604,90 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D, } /* ************************************************************************* */ -void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, - const string& filename) { +void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, + const string &filename) { fstream stream(filename.c_str(), fstream::out); // save 2D poses - for (const auto& key_value : estimate) { - auto p = dynamic_cast*>(&key_value.value); - if (!p) continue; - const Pose2& pose = p->value(); + for (const auto &key_value : estimate) { + auto p = dynamic_cast *>(&key_value.value); + if (!p) + continue; + const Pose2 &pose = p->value(); stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " - << pose.y() << " " << pose.theta() << endl; + << pose.y() << " " << pose.theta() << endl; } // save 3D poses - for(const auto& key_value: estimate) { - auto p = dynamic_cast*>(&key_value.value); - if (!p) continue; - const Pose3& pose = p->value(); + for (const auto &key_value : estimate) { + auto p = dynamic_cast *>(&key_value.value); + if (!p) + continue; + const Pose3 &pose = p->value(); const Point3 t = pose.translation(); const auto q = pose.rotation().toQuaternion(); stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " - << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " - << q.z() << " " << q.w() << endl; + << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << endl; } // save 2D landmarks - for(const auto& key_value: estimate) { - auto p = dynamic_cast*>(&key_value.value); - if (!p) continue; - const Point2& point = p->value(); + for (const auto &key_value : estimate) { + auto p = dynamic_cast *>(&key_value.value); + if (!p) + continue; + const Point2 &point = p->value(); stream << "VERTEX_XY " << key_value.key << " " << point.x() << " " - << point.y() << endl; + << point.y() << endl; } // save 3D landmarks - for(const auto& key_value: estimate) { - auto p = dynamic_cast*>(&key_value.value); - if (!p) continue; - const Point3& point = p->value(); + for (const auto &key_value : estimate) { + auto p = dynamic_cast *>(&key_value.value); + if (!p) + continue; + const Point3 &point = p->value(); stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " " - << point.y() << " " << point.z() << endl; + << point.y() << " " << point.z() << endl; } // save edges (2D or 3D) - for(const auto& factor_: graph) { - boost::shared_ptr > factor = - boost::dynamic_pointer_cast >(factor_); - if (factor){ + for (const auto &factor_ : graph) { + boost::shared_ptr> factor = + boost::dynamic_pointer_cast>(factor_); + if (factor) { SharedNoiseModel model = factor->noiseModel(); boost::shared_ptr gaussianModel = boost::dynamic_pointer_cast(model); - if (!gaussianModel){ + if (!gaussianModel) { model->print("model\n"); throw invalid_argument("writeG2o: invalid noise model!"); } - Matrix Info = gaussianModel->R().transpose() * gaussianModel->R(); + Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R(); Pose2 pose = factor->measured(); //.inverse(); stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " - << pose.x() << " " << pose.y() << " " << pose.theta(); - for (size_t i = 0; i < 3; i++){ - for (size_t j = i; j < 3; j++){ + << pose.x() << " " << pose.y() << " " << pose.theta(); + for (size_t i = 0; i < 3; i++) { + for (size_t j = i; j < 3; j++) { stream << " " << Info(i, j); } } stream << endl; } - boost::shared_ptr< BetweenFactor > factor3D = - boost::dynamic_pointer_cast< BetweenFactor >(factor_); + boost::shared_ptr> factor3D = + boost::dynamic_pointer_cast>(factor_); - if (factor3D){ + if (factor3D) { SharedNoiseModel model = factor3D->noiseModel(); boost::shared_ptr gaussianModel = boost::dynamic_pointer_cast(model); - if (!gaussianModel){ + if (!gaussianModel) { model->print("model\n"); throw invalid_argument("writeG2o: invalid noise model!"); } - Matrix Info = gaussianModel->R().transpose() * gaussianModel->R(); + Matrix6 Info = gaussianModel->R().transpose() * gaussianModel->R(); const Pose3 pose3D = factor3D->measured(); const Point3 p = pose3D.translation(); const auto q = pose3D.rotation().toQuaternion(); @@ -571,14 +695,14 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, << " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w(); - Matrix InfoG2o = I_6x6; + Matrix6 InfoG2o = I_6x6; InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation InfoG2o.block<3, 3>(3, 3) = Info.block<3, 3>(0, 0); // cov rotation InfoG2o.block<3, 3>(0, 3) = Info.block<3, 3>(0, 3); // off diagonal InfoG2o.block<3, 3>(3, 0) = Info.block<3, 3>(3, 0); // off diagonal - for (size_t i = 0; i < 6; i++){ - for (size_t j = i; j < 6; j++){ + for (size_t i = 0; i < 6; i++) { + for (size_t j = i; j < 6; j++) { stream << " " << InfoG2o(i, j); } } @@ -602,7 +726,7 @@ istream &operator>>(istream &is, Quaternion &q) { // parse Rot3 from roll, pitch, yaw istream &operator>>(istream &is, Rot3 &R) { double yaw, pitch, roll; - is >> roll >> pitch >> yaw; // notice order ! + is >> roll >> pitch >> yaw; // notice order ! R = Rot3::Ypr(yaw, pitch, roll); return is; } @@ -648,69 +772,43 @@ map parse3DLandmarks(const string &filename, Key maxNr) { return parseIntoMap(filename, maxNr); } -/* ************************************************************************* */ -// Parse BetweenFactor shared pointers into a vector -template -static vector>> -parseIntoVector(const string &filename, Key maxNr = 0) { - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("parse: can not find file " + filename); - - vector>> result; - string tag; - while (!is.eof()) { - boost::shared_ptr> shared; - is >> shared; - if (shared) { - // optional filter - if (maxNr && (shared->key1() >= maxNr || shared->key1() >= maxNr)) - continue; - result.push_back(shared); - } - is.ignore(LINESIZE, '\n'); - } - return result; -} - /* ************************************************************************* */ // Parse a symmetric covariance matrix (onlyupper-triangular part is stored) istream &operator>>(istream &is, Matrix6 &m) { 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); - m(j,i) = m(i,j); - } + m(j, i) = m(i, j); + } return is; } /* ************************************************************************* */ -istream &operator>>(istream &is, - boost::shared_ptr> &shared) { +struct Measurement3 { + Key id1, id2; + Pose3 pose; + SharedNoiseModel model; +}; + +istream &operator>>(istream &is, boost::optional &edge) { string tag; is >> tag; - Matrix6 m; if (tag == "EDGE3") { Key id1, id2; double x, y, z; Rot3 R; - is >> id1 >> id2 >> x >> y >> z >> R; - Pose3 pose(R, {x, y, z}); - - is >> m; - SharedNoiseModel model = noiseModel::Gaussian::Information(m); - shared.reset(new BetweenFactor(id1, id2, pose, model)); + is >> id1 >> id2 >> x >> y >> z >> R >> m; + edge.reset( + {id1, id2, Pose3(R, {x, y, z}), noiseModel::Gaussian::Information(m)}); } if (tag == "EDGE_SE3:QUAT") { Key id1, id2; double x, y, z; Quaternion q; - is >> id1 >> id2 >> x >> y >> z >> q; - Pose3 pose(q, {x, y, z}); + is >> id1 >> id2 >> x >> y >> z >> q >> m; // EDGE_SE3:QUAT stores covariance in t,R order, unlike GTSAM: - is >> m; Matrix6 mgtsam; mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation @@ -718,47 +816,67 @@ istream &operator>>(istream &is, mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - shared.reset(new BetweenFactor(id1, id2, pose, model)); + edge.reset({id1, id2, Pose3(q, {x, y, z}), + noiseModel::Gaussian::Information(mgtsam)}); } return is; } +/* ************************************************************************* */ +// Small functor to process the edge into a BetweenFactor::shared_ptr +struct ProcessPose3 { + // The arguments + Key maxNr = 0; + SharedNoiseModel model; + boost::shared_ptr sampler; + + // The actual function + BetweenFactor::shared_ptr operator()(const Measurement3 &edge) { + // optional filter + if (maxNr && (edge.id1 >= maxNr || edge.id2 >= maxNr)) + return nullptr; + + // Get pose and optionally add noise + Pose3 T12 = edge.pose; + if (sampler) + T12 = T12.retract(sampler->sample()); + + // Create factor + return boost::make_shared>(edge.id1, edge.id2, T12, + edge.model); + } +}; + /* ************************************************************************* */ BetweenFactorPose3s parse3DFactors(const string &filename, const noiseModel::Diagonal::shared_ptr &corruptingNoise, Key maxNr) { - auto factors = parseIntoVector(filename, maxNr); - + ProcessPose3 process; + process.maxNr = maxNr; if (corruptingNoise) { - Sampler sampler(corruptingNoise); - for (auto factor : factors) { - auto pose = factor->measured(); - factor.reset(new BetweenFactor(factor->key1(), factor->key2(), - pose.retract(sampler.sample()), - factor->noiseModel())); - } + process.sampler = createSampler(corruptingNoise); } - - return factors; + return parseToVector::shared_ptr>(filename, + process); } /* ************************************************************************* */ -GraphAndValues load3D(const string& filename) { +GraphAndValues load3D(const string &filename) { const auto factors = parse3DFactors(filename); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); - for (const auto& factor : factors) { + for (const auto &factor : factors) { graph->push_back(factor); } Values::shared_ptr initial(new Values); const auto poses = parse3DPoses(filename); - for (const auto& key_pose : poses) { + for (const auto &key_pose : poses) { initial->insert(key_pose.first, key_pose.second); } const auto landmarks = parse3DLandmarks(filename); - for (const auto& key_landmark : landmarks) { + for (const auto &key_landmark : landmarks) { initial->insert(key_landmark.first, key_landmark.second); } @@ -766,7 +884,8 @@ GraphAndValues load3D(const string& filename) { } /* ************************************************************************* */ -Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL +Rot3 openGLFixedRotation() { // this is due to different convention for + // cameras in gtsam and openGL /* R = [ 1 0 0 * 0 -1 0 * 0 0 -1] @@ -779,7 +898,7 @@ Rot3 openGLFixedRotation() { // this is due to different convention for cameras } /* ************************************************************************* */ -Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) { +Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) { Rot3 R90 = openGLFixedRotation(); Rot3 wRc = (R.inverse()).compose(R90); @@ -788,7 +907,7 @@ Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) { } /* ************************************************************************* */ -Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) { +Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) { Rot3 R90 = openGLFixedRotation(); Rot3 cRw_openGL = R90.compose(R.inverse()); Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); @@ -796,13 +915,13 @@ Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) { } /* ************************************************************************* */ -Pose3 gtsam2openGL(const Pose3& PoseGTSAM) { +Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) { return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), - PoseGTSAM.z()); + PoseGTSAM.z()); } /* ************************************************************************* */ -bool readBundler(const string& filename, SfmData &data) { +bool readBundler(const string &filename, SfmData &data) { // Load the data file ifstream is(filename.c_str(), ifstream::in); if (!is) { @@ -837,7 +956,7 @@ bool readBundler(const string& filename, SfmData &data) { // Check for all-zero R, in which case quit if (r11 == 0 && r12 == 0 && r13 == 0) { cout << "Error in readBundler: zero rotation matrix for pose " << i - << endl; + << endl; return false; } @@ -889,7 +1008,7 @@ bool readBundler(const string& filename, SfmData &data) { } /* ************************************************************************* */ -bool readBAL(const string& filename, SfmData &data) { +bool readBAL(const string &filename, SfmData &data) { // Load the data file ifstream is(filename.c_str(), ifstream::in); if (!is) { @@ -937,7 +1056,7 @@ bool readBAL(const string& filename, SfmData &data) { // Get the 3D position float x, y, z; is >> x >> y >> z; - SfmTrack& track = data.tracks[j]; + SfmTrack &track = data.tracks[j]; track.p = Point3(x, y, z); track.r = 0.4f; track.g = 0.4f; @@ -949,7 +1068,7 @@ bool readBAL(const string& filename, SfmData &data) { } /* ************************************************************************* */ -bool writeBAL(const string& filename, SfmData &data) { +bool writeBAL(const string &filename, SfmData &data) { // Open the output file ofstream os; os.open(filename.c_str()); @@ -967,29 +1086,32 @@ bool writeBAL(const string& filename, SfmData &data) { // Write observations os << data.number_cameras() << " " << data.number_tracks() << " " - << nrObservations << endl; + << nrObservations << endl; os << endl; for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j - const SfmTrack& track = data.tracks[j]; + const SfmTrack &track = data.tracks[j]; - for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j + for (size_t k = 0; k < track.number_measurements(); + k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id double u0 = data.cameras[i].calibration().u0(); double v0 = data.cameras[i].calibration().v0(); if (u0 != 0 || v0 != 0) { - cout - << "writeBAL has not been tested for calibration with nonzero (u0,v0)" - << endl; + cout << "writeBAL has not been tested for calibration with nonzero " + "(u0,v0)" + << endl; } - double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin - double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin + double pixelBALx = track.measurements[k].second.x() - + u0; // center of image is the origin + double pixelBALy = -(track.measurements[k].second.y() - + v0); // center of image is the origin Point2 pixelMeasurement(pixelBALx, pixelBALy); - os << i /*camera id*/<< " " << j /*point id*/<< " " - << pixelMeasurement.x() /*u of the pixel*/<< " " - << pixelMeasurement.y() /*v of the pixel*/<< endl; + os << i /*camera id*/ << " " << j /*point id*/ << " " + << pixelMeasurement.x() /*u of the pixel*/ << " " + << pixelMeasurement.y() /*v of the pixel*/ << endl; } } os << endl; @@ -1022,15 +1144,17 @@ bool writeBAL(const string& filename, SfmData &data) { return true; } -bool writeBALfromValues(const string& filename, const SfmData &data, - Values& values) { +bool writeBALfromValues(const string &filename, const SfmData &data, + Values &values) { using Camera = PinholeCamera; SfmData dataValues = data; // Store poses or cameras in SfmData size_t nrPoses = values.count(); - if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses - for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera + if (nrPoses == + dataValues.number_cameras()) { // we only estimated camera poses + for (size_t i = 0; i < dataValues.number_cameras(); + i++) { // for each camera Key poseKey = symbol('x', i); Pose3 pose = values.at(poseKey); Cal3Bundler K = dataValues.cameras[i].calibration(); @@ -1039,29 +1163,29 @@ bool writeBALfromValues(const string& filename, const SfmData &data, } } else { size_t nrCameras = values.count(); - if (nrCameras == dataValues.number_cameras()) { // we only estimated camera poses and calibration - for (size_t i = 0; i < nrCameras; i++) { // for each camera - Key cameraKey = i; // symbol('c',i); + if (nrCameras == dataValues.number_cameras()) { // we only estimated camera + // poses and calibration + for (size_t i = 0; i < nrCameras; i++) { // for each camera + Key cameraKey = i; // symbol('c',i); Camera camera = values.at(cameraKey); dataValues.cameras[i] = camera; } } else { - cout - << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras " - << dataValues.number_cameras() << ") and values (#cameras " - << nrPoses << ", #poses " << nrCameras << ")!!" - << endl; + cout << "writeBALfromValues: different number of cameras in " + "SfM_dataValues (#cameras " + << dataValues.number_cameras() << ") and values (#cameras " + << nrPoses << ", #poses " << nrCameras << ")!!" << endl; return false; } } // Store 3D points in SfmData - size_t nrPoints = values.count(), nrTracks = dataValues.number_tracks(); + size_t nrPoints = values.count(), + nrTracks = dataValues.number_tracks(); if (nrPoints != nrTracks) { - cout - << "writeBALfromValues: different number of points in SfM_dataValues (#points= " - << nrTracks << ") and values (#points " - << nrPoints << ")!!" << endl; + cout << "writeBALfromValues: different number of points in " + "SfM_dataValues (#points= " + << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl; } for (size_t j = 0; j < nrTracks; j++) { // for each point @@ -1073,7 +1197,7 @@ bool writeBALfromValues(const string& filename, const SfmData &data, dataValues.tracks[j].r = 1.0; dataValues.tracks[j].g = 0.0; dataValues.tracks[j].b = 0.0; - dataValues.tracks[j].p = Point3(0,0,0); + dataValues.tracks[j].p = Point3(0, 0, 0); } } @@ -1081,22 +1205,22 @@ bool writeBALfromValues(const string& filename, const SfmData &data, return writeBAL(filename, dataValues); } -Values initialCamerasEstimate(const SfmData& db) { +Values initialCamerasEstimate(const SfmData &db) { Values initial; size_t i = 0; // NO POINTS: j = 0; - for(const SfmCamera& camera: db.cameras) + for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera); return initial; } -Values initialCamerasAndPointsEstimate(const SfmData& db) { +Values initialCamerasAndPointsEstimate(const SfmData &db) { Values initial; size_t i = 0, j = 0; - for(const SfmCamera& camera: db.cameras) + for (const SfmCamera &camera : db.cameras) initial.insert((i++), camera); - for(const SfmTrack& track: db.tracks) + for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p); return initial; } -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index caef96cf4..5b92800af 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -77,16 +77,6 @@ typedef std::pair IndexedPose; typedef std::pair IndexedLandmark; typedef std::pair, Pose2> IndexedEdge; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 -/** - * Parse TORO/G2O vertex "id x y yaw" - * @param is input stream - * @param tag string parsed from input stream, will only parse if vertex type - */ -GTSAM_EXPORT boost::optional parseVertex(std::istream& is, - const std::string& tag); -#endif - /** * Parse TORO/G2O vertex "id x y yaw" * @param is input stream @@ -118,7 +108,8 @@ using BetweenFactorPose2s = /// Parse edges in 2D g2o graph file into a set of BetweenFactors. GTSAM_EXPORT BetweenFactorPose2s parse2DFactors( const std::string &filename, - const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr); + const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr, + Key maxNr = 0); /// Parse vertices in 2D g2o graph file into a map of Pose2s. GTSAM_EXPORT std::map parse2DPoses(const std::string &filename, @@ -343,4 +334,15 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); */ GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +/** + * Parse TORO/G2O vertex "id x y yaw" + * @param is input stream + * @param tag string parsed from input stream, will only parse if vertex type + */ +GTSAM_EXPORT boost::optional parseVertex(std::istream &is, + const std::string &tag) { + return parseVertexPose(is, tag); +} +#endif } // namespace gtsam diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 6ee44cfd6..bc65dc351 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -104,13 +104,13 @@ TEST(dataSet, load2D) { boost::dynamic_pointer_cast>(graph->at(0)); EXPECT(assert_equal(expected, *actual)); - // // Check factor parsing - // const auto actualFactors = parse2DFactors(filename); - // for (size_t i : {0, 1, 2, 3, 4, 5}) { - // EXPECT(assert_equal( - // *boost::dynamic_pointer_cast>(graph->at(i)), - // *actualFactors[i], 1e-5)); - // } + // Check factor parsing + const auto actualFactors = parse2DFactors(filename); + for (size_t i : {0, 1, 2, 3, 4, 5}) { + EXPECT(assert_equal( + *boost::dynamic_pointer_cast>(graph->at(i)), + *actualFactors[i], 1e-5)); + } // Check pose parsing const auto actualPoses = parse2DPoses(filename); From aa3c0f8c5d679e1c5fb7fd16ae8078243e810672 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 12 Aug 2020 23:59:57 -0400 Subject: [PATCH 03/10] refactored load2d --- gtsam/slam/dataset.cpp | 189 ++++++++++++++++++----------------------- gtsam/slam/dataset.h | 26 +++--- 2 files changed, 96 insertions(+), 119 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 29162dafb..af8904e49 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -108,10 +108,10 @@ string createRewrittenFileName(const string &name) { } /* ************************************************************************* */ -GraphAndValues load2D(pair dataset, Key maxNr, +GraphAndValues load2D(pair dataset, Key maxKey, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - return load2D(dataset.first, dataset.second, maxNr, addNoise, smart, + return load2D(dataset.first, dataset.second, maxKey, addNoise, smart, noiseFormat, kernelFunctionType); } @@ -199,16 +199,6 @@ createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat, } } -/* ************************************************************************* */ -// Read noise parameters and interpret them according to flags -static SharedNoiseModel readNoiseModel(istream &is, bool smart, - NoiseFormat noiseFormat, - KernelFunctionType kernelFunctionType) { - Vector6 v; - is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); - return createNoiseModel(v, smart, noiseFormat, kernelFunctionType); -} - /* ************************************************************************* */ boost::optional parseVertexPose(istream &is, const string &tag) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { @@ -237,7 +227,7 @@ boost::optional parseVertexLandmark(istream &is, /* ************************************************************************* */ // Parse types T into a Key-indexed map template -static map parseIntoMap(const string &filename, Key maxNr = 0) { +static map parseIntoMap(const string &filename, Key maxKey = 0) { ifstream is(filename.c_str()); if (!is) throw invalid_argument("parse: can not find file " + filename); @@ -249,7 +239,7 @@ static map parseIntoMap(const string &filename, Key maxNr = 0) { is >> indexed_t; if (indexed_t) { // optional filter - if (maxNr && indexed_t->first >= maxNr) + if (maxKey && indexed_t->first >= maxKey) continue; result.insert(*indexed_t); } @@ -266,8 +256,8 @@ istream &operator>>(istream &is, boost::optional &indexed) { return is; } -map parse2DPoses(const string &filename, Key maxNr) { - return parseIntoMap(filename, maxNr); +map parse2DPoses(const string &filename, Key maxKey) { + return parseIntoMap(filename, maxKey); } /* ************************************************************************* */ @@ -278,8 +268,8 @@ istream &operator>>(istream &is, boost::optional &indexed) { return is; } -map parse2DLandmarks(const string &filename, Key maxNr) { - return parseIntoMap(filename, maxNr); +map parse2DLandmarks(const string &filename, Key maxKey) { + return parseIntoMap(filename, maxKey); } /* ************************************************************************* */ @@ -360,19 +350,26 @@ boost::shared_ptr createSampler(const SharedNoiseModel &model) { // Small functor to process the edge into a BetweenFactor::shared_ptr struct ProcessPose2 { // The arguments - Key maxNr = 0; + Key maxKey; SharedNoiseModel model; boost::shared_ptr sampler; // Arguments for parsing noise model - bool smart = true; - NoiseFormat noiseFormat = NoiseFormatAUTO; - KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE; + bool smart; + NoiseFormat noiseFormat; + KernelFunctionType kernelFunctionType; + + ProcessPose2(Key maxKey = 0, SharedNoiseModel model = nullptr, + boost::shared_ptr sampler = nullptr, bool smart = true, + NoiseFormat noiseFormat = NoiseFormatAUTO, + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE) + : maxKey(maxKey), model(model), sampler(sampler), smart(smart), + noiseFormat(noiseFormat), kernelFunctionType(kernelFunctionType) {} // The actual function BetweenFactor::shared_ptr operator()(const Measurement2 &edge) { // optional filter - if (maxNr && (edge.id1 >= maxNr || edge.id2 >= maxNr)) + if (maxKey && (edge.id1 >= maxKey || edge.id2 >= maxKey)) return nullptr; // Get pose and optionally add noise @@ -394,90 +391,32 @@ struct ProcessPose2 { BetweenFactorPose2s parse2DFactors(const string &filename, const noiseModel::Diagonal::shared_ptr &corruptingNoise, - Key maxNr) { - ProcessPose2 process; - process.maxNr = maxNr; - if (corruptingNoise) { - process.sampler = createSampler(corruptingNoise); - } + Key maxKey) { + ProcessPose2 process(maxKey, nullptr, + corruptingNoise ? createSampler(corruptingNoise) + : nullptr); + return parseToVector::shared_ptr>(filename, process); } /* ************************************************************************* */ -GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, - bool addNoise, bool smart, NoiseFormat noiseFormat, - KernelFunctionType kernelFunctionType) { - - Values::shared_ptr initial(new Values); - - const auto poses = parse2DPoses(filename, maxNr); - for (const auto &key_pose : poses) { - initial->insert(key_pose.first, key_pose.second); - } - const auto landmarks = parse2DLandmarks(filename, maxNr); - for (const auto &key_landmark : landmarks) { - initial->insert(key_landmark.first, key_landmark.second); - } - +static void parseOthers(const string &filename, Key maxKey, + NonlinearFactorGraph::shared_ptr graph, + Values::shared_ptr initial) { + // Do a pass to get other types of measurements ifstream is(filename.c_str()); if (!is) throw invalid_argument("load2D: can not find file " + filename); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); - - // If asked, create a sampler with random number generator - boost::shared_ptr sampler; - if (addNoise) { - noiseModel::Diagonal::shared_ptr noise; - if (model) - noise = boost::dynamic_pointer_cast(model); - if (!noise) - throw invalid_argument( - "gtsam::load2D: invalid noise model for adding noise" - "(current version assumes diagonal noise model)!"); - sampler.reset(new Sampler(noise)); - } - - // Parse the pose constraints Key id1, id2; - bool haveLandmark = false; - const bool useModelInFile = !model; while (!is.eof()) { string tag; if (!(is >> tag)) break; - auto between_pose = parseEdge(is, tag); - if (between_pose) { - std::tie(id1, id2) = between_pose->first; - Pose2 l1Xl2 = between_pose->second; - - // read noise model - SharedNoiseModel modelInFile = - readNoiseModel(is, smart, noiseFormat, kernelFunctionType); - - // optional filter - if (maxNr && (id1 >= maxNr || id2 >= maxNr)) - continue; - - if (useModelInFile) - model = modelInFile; - - if (addNoise) - l1Xl2 = l1Xl2.retract(sampler->sample()); - - // Insert vertices if pure odometry file - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) - initial->insert(id2, initial->at(id1) * l1Xl2); - - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, l1Xl2, model)); - graph->push_back(factor); - } // Parse measurements + bool haveLandmark = false; double bearing, range, bearing_std, range_std; // A bearing-range measurement @@ -485,8 +424,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; } - // A landmark measurement, TODO Frank says: don't know why is converted to - // bearing-range + // A landmark measurement, converted to bearing-range if (tag == "LANDMARK") { double lmx, lmy; double v1, v2, v3; @@ -521,7 +459,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, if (tag == "LANDMARK" || tag == "BR") { // optional filter - if (maxNr && id1 >= maxNr) + if (maxKey && id1 >= maxKey) continue; // Create noise model @@ -545,14 +483,53 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, } is.ignore(LINESIZE, '\n'); } +} + +/* ************************************************************************* */ +GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxKey, + bool addNoise, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + + Values::shared_ptr initial(new Values); + + const auto poses = parse2DPoses(filename, maxKey); + for (const auto &key_pose : poses) { + initial->insert(key_pose.first, key_pose.second); + } + const auto landmarks = parse2DLandmarks(filename, maxKey); + for (const auto &key_landmark : landmarks) { + initial->insert(key_landmark.first, key_landmark.second); + } + + // Parse the pose constraints + ProcessPose2 process(maxKey, model, addNoise ? createSampler(model) : nullptr, + smart, noiseFormat, kernelFunctionType); + auto factors = parseToVector::shared_ptr>( + filename, process); + + // Add factors into the graph and add poses if necessary + auto graph = boost::make_shared(); + for (const auto &f : factors) { + graph->push_back(f); + + // Insert vertices if pure odometry file + Key id1 = f->key1(), id2 = f->key2(); + if (!initial->exists(id1)) + initial->insert(id1, Pose2()); + if (!initial->exists(id2)) + initial->insert(id2, initial->at(id1) * f->measured()); + } + + // Do a pass to parse landmarks and bearing-range measurements + parseOthers(filename, maxKey, graph, initial); return make_pair(graph, initial); } /* ************************************************************************* */ GraphAndValues load2D_robust(const string &filename, - noiseModel::Base::shared_ptr &model, Key maxNr) { - return load2D(filename, model, maxNr); + noiseModel::Base::shared_ptr &model, Key maxKey) { + return load2D(filename, model, maxKey); } /* ************************************************************************* */ @@ -595,10 +572,10 @@ GraphAndValues readG2o(const string &g2oFile, const bool is3D, return load3D(g2oFile); } else { // just call load2D - Key maxNr = 0; + Key maxKey = 0; bool addNoise = false; bool smart = true; - return load2D(g2oFile, SharedNoiseModel(), maxNr, addNoise, smart, + return load2D(g2oFile, SharedNoiseModel(), maxKey, addNoise, smart, NoiseFormatG2O, kernelFunctionType); } } @@ -751,8 +728,8 @@ istream &operator>>(istream &is, boost::optional> &indexed) { return is; } -map parse3DPoses(const string &filename, Key maxNr) { - return parseIntoMap(filename, maxNr); +map parse3DPoses(const string &filename, Key maxKey) { + return parseIntoMap(filename, maxKey); } /* ************************************************************************* */ @@ -768,8 +745,8 @@ istream &operator>>(istream &is, boost::optional> &indexed) { return is; } -map parse3DLandmarks(const string &filename, Key maxNr) { - return parseIntoMap(filename, maxNr); +map parse3DLandmarks(const string &filename, Key maxKey) { + return parseIntoMap(filename, maxKey); } /* ************************************************************************* */ @@ -826,14 +803,14 @@ istream &operator>>(istream &is, boost::optional &edge) { // Small functor to process the edge into a BetweenFactor::shared_ptr struct ProcessPose3 { // The arguments - Key maxNr = 0; + Key maxKey = 0; SharedNoiseModel model; boost::shared_ptr sampler; // The actual function BetweenFactor::shared_ptr operator()(const Measurement3 &edge) { // optional filter - if (maxNr && (edge.id1 >= maxNr || edge.id2 >= maxNr)) + if (maxKey && (edge.id1 >= maxKey || edge.id2 >= maxKey)) return nullptr; // Get pose and optionally add noise @@ -851,9 +828,9 @@ struct ProcessPose3 { BetweenFactorPose3s parse3DFactors(const string &filename, const noiseModel::Diagonal::shared_ptr &corruptingNoise, - Key maxNr) { + Key maxKey) { ProcessPose3 process; - process.maxNr = maxNr; + process.maxKey = maxKey; if (corruptingNoise) { process.sampler = createSampler(corruptingNoise); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 5b92800af..61eb9256e 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -109,15 +109,15 @@ using BetweenFactorPose2s = GTSAM_EXPORT BetweenFactorPose2s parse2DFactors( const std::string &filename, const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr, - Key maxNr = 0); + Key maxKey = 0); /// Parse vertices in 2D g2o graph file into a map of Pose2s. GTSAM_EXPORT std::map parse2DPoses(const std::string &filename, - Key maxNr = 0); + Key maxKey = 0); /// Parse landmarks in 2D g2o graph file into a map of Point2s. GTSAM_EXPORT std::map parse2DLandmarks(const string &filename, - Key maxNr = 0); + Key maxKey = 0); /// Return type for load functions using GraphAndValues = @@ -126,12 +126,12 @@ using GraphAndValues = /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] - * @param maxNr if non-zero cut out vertices >= maxNr + * @param maxKey if non-zero cut out vertices >= maxKey * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ GTSAM_EXPORT GraphAndValues load2D( - std::pair dataset, Key maxNr = 0, + std::pair dataset, Key maxKey = 0, bool addNoise = false, bool smart = true, // NoiseFormat noiseFormat = NoiseFormatAUTO, @@ -141,7 +141,7 @@ GTSAM_EXPORT GraphAndValues load2D( * Load TORO/G2O style graph files * @param filename * @param model optional noise model to use instead of one specified by file - * @param maxNr if non-zero cut out vertices >= maxNr + * @param maxKey if non-zero cut out vertices >= maxKey * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model * @param noiseFormat how noise parameters are stored @@ -149,13 +149,13 @@ GTSAM_EXPORT GraphAndValues load2D( * @return graph and initial values */ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, - SharedNoiseModel model = SharedNoiseModel(), Key maxNr = 0, bool addNoise = + SharedNoiseModel model = SharedNoiseModel(), Key maxKey = 0, bool addNoise = false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, - noiseModel::Base::shared_ptr& model, Key maxNr = 0); + noiseModel::Base::shared_ptr& model, Key maxKey = 0); /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, @@ -188,15 +188,15 @@ using BetweenFactorPose3s = std::vector::shared_ptr> GTSAM_EXPORT BetweenFactorPose3s parse3DFactors( const std::string &filename, const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr, - Key maxNr = 0); + Key maxKey = 0); /// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s. GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, - Key maxNr = 0); + Key maxKey = 0); /// Parse landmarks in 3D g2o graph file into a map of Point3s. GTSAM_EXPORT std::map parse3DLandmarks(const std::string &filename, - Key maxNr = 0); + Key maxKey = 0); /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); @@ -340,8 +340,8 @@ GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); * @param is input stream * @param tag string parsed from input stream, will only parse if vertex type */ -GTSAM_EXPORT boost::optional parseVertex(std::istream &is, - const std::string &tag) { +inline boost::optional parseVertex(std::istream &is, + const std::string &tag) { return parseVertexPose(is, tag); } #endif From e62d04ce2715f9fa913ce0979447334c3eb27855 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 13 Aug 2020 20:38:58 -0400 Subject: [PATCH 04/10] Templated parse methods --- gtsam/nonlinear/Values.h | 2 +- gtsam/sam/BearingRangeFactor.h | 15 +- gtsam/sfm/BinaryMeasurement.h | 93 ++-- gtsam/slam/dataset.cpp | 763 +++++++++++++++++-------------- gtsam/slam/dataset.h | 79 ++-- gtsam/slam/tests/testDataset.cpp | 26 +- timing/timeFrobeniusFactor.cpp | 2 +- 7 files changed, 530 insertions(+), 450 deletions(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index aadbcf394..b49188b68 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -397,7 +397,7 @@ namespace gtsam { template size_t count() const { size_t i = 0; - for (const auto& key_value : *this) { + for (const auto key_value : *this) { if (dynamic_cast*>(&key_value.value)) ++i; } diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index af7b47446..b8e231915 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -42,12 +42,19 @@ class BearingRangeFactor public: typedef boost::shared_ptr shared_ptr; - /// default constructor + /// Default constructor BearingRangeFactor() {} - /// primary constructor - BearingRangeFactor(Key key1, Key key2, const B& measuredBearing, - const R& measuredRange, const SharedNoiseModel& model) + /// Construct from BearingRange instance + BearingRangeFactor(Key key1, Key key2, const T &bearingRange, + const SharedNoiseModel &model) + : Base({key1, key2}, model, T(bearingRange)) { + this->initialize(expression({key1, key2})); + } + + /// Construct from separate bearing and range + BearingRangeFactor(Key key1, Key key2, const B &measuredBearing, + const R &measuredRange, const SharedNoiseModel &model) : Base({key1, key2}, model, T(measuredBearing, measuredRange)) { this->initialize(expression({key1, key2})); } diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index c4b4a9804..c525c1b9e 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -15,78 +15,69 @@ * @file BinaryMeasurement.h * @author Akshay Krishnan * @date July 2020 - * @brief Binary measurement represents a measurement between two keys in a graph. - * A binary measurement is similar to a BetweenFactor, except that it does not contain - * an error function. It is a measurement (along with a noise model) from one key to - * another. Note that the direction is important. A measurement from key1 to key2 is not - * the same as the same measurement from key2 to key1. + * @brief Binary measurement represents a measurement between two keys in a + * graph. A binary measurement is similar to a BetweenFactor, except that it + * does not contain an error function. It is a measurement (along with a noise + * model) from one key to another. Note that the direction is important. A + * measurement from key1 to key2 is not the same as the same measurement from + * key2 to key1. */ -#include - #include -#include -#include - +#include #include #include +#include +#include + namespace gtsam { -template -class BinaryMeasurement { - // Check that VALUE type is testable - BOOST_CONCEPT_ASSERT((IsTestable)); - - public: - typedef VALUE T; +template class BinaryMeasurement : public Factor { + // Check that T type is testable + BOOST_CONCEPT_ASSERT((IsTestable)); +public: // shorthand for a smart pointer to a measurement - typedef typename boost::shared_ptr shared_ptr; + using shared_ptr = typename boost::shared_ptr; - private: - Key key1_, key2_; /** Keys */ +private: + T measured_; ///< The measurement + SharedNoiseModel noiseModel_; ///< Noise model - VALUE measured_; /** The measurement */ +public: + BinaryMeasurement(Key key1, Key key2, const T &measured, + const SharedNoiseModel &model = nullptr) + : Factor(std::vector({key1, key2})), measured_(measured), + noiseModel_(model) {} - SharedNoiseModel noiseModel_; /** Noise model */ - - public: - /** Constructor */ - BinaryMeasurement(Key key1, Key key2, const VALUE &measured, - const SharedNoiseModel &model = nullptr) : - key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) { - } - - Key key1() const { return key1_; } - - Key key2() const { return key2_; } + /// @name Standard Interface + /// @{ + Key key1() const { return keys_[0]; } + Key key2() const { return keys_[1]; } + const T &measured() const { return measured_; } const SharedNoiseModel &noiseModel() const { return noiseModel_; } - /** implement functions needed for Testable */ + /// @} + /// @name Testable + /// @{ - /** print */ - void print(const std::string &s, const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "BinaryMeasurement(" - << keyFormatter(this->key1()) << "," + void print(const std::string &s, + const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; traits::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } - /** equals */ bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const { - const BinaryMeasurement *e = dynamic_cast *> (&expected); - return e != nullptr && key1_ == e->key1_ && - key2_ == e->key2_ - && traits::Equals(this->measured_, e->measured_, tol) && - noiseModel_->equals(*expected.noiseModel()); + const BinaryMeasurement *e = + dynamic_cast *>(&expected); + return e != nullptr && Factor::equals(*e) && + traits::Equals(this->measured_, e->measured_, tol) && + noiseModel_->equals(*expected.noiseModel()); } - - /** return the measured value */ - VALUE measured() const { - return measured_; - } -}; // \class BetweenMeasurement -} \ No newline at end of file + /// @} +}; +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index af8904e49..11473d084 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -108,11 +108,94 @@ string createRewrittenFileName(const string &name) { } /* ************************************************************************* */ -GraphAndValues load2D(pair dataset, Key maxKey, - bool addNoise, bool smart, NoiseFormat noiseFormat, - KernelFunctionType kernelFunctionType) { - return load2D(dataset.first, dataset.second, maxKey, addNoise, smart, - noiseFormat, kernelFunctionType); +// Parse a file by calling the parse(is, tag) function for every line. +template +using Parser = + std::function(istream &is, const string &tag)>; + +template +static void parseLines(const string &filename, Parser parse) { + ifstream is(filename.c_str()); + if (!is) + throw invalid_argument("parse: can not find file " + filename); + while (!is.eof()) { + string tag; + is >> tag; + parse(is, tag); // ignore return value + is.ignore(LINESIZE, '\n'); + } +} + +/* ************************************************************************* */ +// Parse types T into a Key-indexed map +template +map parseToMap(const string &filename, Parser> parse, + Key maxKey) { + ifstream is(filename.c_str()); + if (!is) + throw invalid_argument("parse: can not find file " + filename); + + map result; + Parser> emplace = [&result, parse](istream &is, + const string &tag) { + if (auto t = parse(is, tag)) + result.emplace(*t); + return boost::none; + }; + parseLines(filename, emplace); + return result; +} + +/* ************************************************************************* */ +// Parse a file and push results on a vector +// TODO(frank): do we need this *and* parseMeasurements? +template +static vector parseToVector(const string &filename, Parser parse) { + vector result; + Parser add = [&result, parse](istream &is, const string &tag) { + if (auto t = parse(is, tag)) + result.push_back(*t); + return boost::none; + }; + parseLines(filename, add); + return result; +} + +/* ************************************************************************* */ +boost::optional parseVertexPose(istream &is, const string &tag) { + if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { + Key id; + double x, y, yaw; + is >> id >> x >> y >> yaw; + return IndexedPose(id, Pose2(x, y, yaw)); + } else { + return boost::none; + } +} + +template <> +std::map parseVariables(const std::string &filename, + Key maxKey) { + return parseToMap(filename, parseVertexPose, maxKey); +} + +/* ************************************************************************* */ +boost::optional parseVertexLandmark(istream &is, + const string &tag) { + if (tag == "VERTEX_XY") { + Key id; + double x, y; + is >> id >> x >> y; + return IndexedLandmark(id, Point2(x, y)); + } else { + return boost::none; + } +} + +template <> +std::map parseVariables(const std::string &filename, + Key maxKey) { + return parseToMap(filename, parseVertexLandmark, maxKey); } /* ************************************************************************* */ @@ -199,107 +282,6 @@ createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat, } } -/* ************************************************************************* */ -boost::optional parseVertexPose(istream &is, const string &tag) { - if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { - Key id; - double x, y, yaw; - is >> id >> x >> y >> yaw; - return IndexedPose(id, Pose2(x, y, yaw)); - } else { - return boost::none; - } -} - -/* ************************************************************************* */ -boost::optional parseVertexLandmark(istream &is, - const string &tag) { - if (tag == "VERTEX_XY") { - Key id; - double x, y; - is >> id >> x >> y; - return IndexedLandmark(id, Point2(x, y)); - } else { - return boost::none; - } -} - -/* ************************************************************************* */ -// Parse types T into a Key-indexed map -template -static map parseIntoMap(const string &filename, Key maxKey = 0) { - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("parse: can not find file " + filename); - - map result; - string tag; - while (!is.eof()) { - boost::optional> indexed_t; - is >> indexed_t; - if (indexed_t) { - // optional filter - if (maxKey && indexed_t->first >= maxKey) - continue; - result.insert(*indexed_t); - } - is.ignore(LINESIZE, '\n'); - } - return result; -} - -/* ************************************************************************* */ -istream &operator>>(istream &is, boost::optional &indexed) { - string tag; - is >> tag; - indexed = parseVertexPose(is, tag); - return is; -} - -map parse2DPoses(const string &filename, Key maxKey) { - return parseIntoMap(filename, maxKey); -} - -/* ************************************************************************* */ -istream &operator>>(istream &is, boost::optional &indexed) { - string tag; - is >> tag; - indexed = parseVertexLandmark(is, tag); - return is; -} - -map parse2DLandmarks(const string &filename, Key maxKey) { - return parseIntoMap(filename, maxKey); -} - -/* ************************************************************************* */ -// Parse a file by first parsing the `Parsed` type and then processing it with -// the supplied `process` function. Result is put in a vector evaluates to true. -// Requires: a stream >> operator for boost::optional -template -static vector -parseToVector(const string &filename, - const std::function process) { - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("parse: can not find file " + filename); - - vector result; - string tag; - while (!is.eof()) { - boost::optional parsed; - is >> parsed; - if (parsed) { - Output factor = process(*parsed); // can return nullptr - if (factor) { - result.push_back(factor); - } - } - is.ignore(LINESIZE, '\n'); - } - return result; -} - /* ************************************************************************* */ boost::optional parseEdge(istream &is, const string &tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || @@ -315,29 +297,74 @@ boost::optional parseEdge(istream &is, const string &tag) { } /* ************************************************************************* */ -struct Measurement2 { - Key id1, id2; - Pose2 pose; - Vector6 v; // 6 noise model parameters for edge -}; - -istream &operator>>(istream &is, boost::optional &edge) { - string tag; - is >> tag; - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || - (tag == "ODOMETRY")) { - Key id1, id2; - double x, y, yaw; - Vector6 v; - is >> id1 >> id2 >> x >> y >> yaw >> // - v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); - edge.reset({id1, id2, Pose2(x, y, yaw), v}); - } - return is; -} +// Measurement parsers are implemented as a functor, as they has several options +// to filter and create the measurement model. +template struct ParseMeasurement; /* ************************************************************************* */ -// Create a sampler +// Converting from Measurement to BetweenFactor is generic +template struct ParseFactor : ParseMeasurement { + ParseFactor(const ParseMeasurement &parent) + : ParseMeasurement(parent) {} + + // We parse a measurement then convert + typename boost::optional::shared_ptr> + operator()(istream &is, const string &tag) { + if (auto m = ParseMeasurement::operator()(is, tag)) + return boost::make_shared>( + m->key1(), m->key2(), m->measured(), m->noiseModel()); + else + return boost::none; + } +}; + +/* ************************************************************************* */ +// Pose2 measurement parser +template <> struct ParseMeasurement { + // The arguments + boost::shared_ptr sampler; + Key maxKey; + + // For noise model creation + bool smart; + NoiseFormat noiseFormat; + KernelFunctionType kernelFunctionType; + + // If this is not null, will use instead of parsed model: + SharedNoiseModel model; + + // The actual parser + boost::optional> operator()(istream &is, + const string &tag) { + auto edge = parseEdge(is, tag); + if (!edge) + return boost::none; + + // parse noise model + Vector6 v; + is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); + + // optional filter + Key id1, id2; + tie(id1, id2) = edge->first; + if (maxKey && (id1 >= maxKey || id2 >= maxKey)) + return boost::none; + + // Get pose and optionally add noise + Pose2 &pose = edge->second; + if (sampler) + pose = pose.retract(sampler->sample()); + + // emplace measurement + auto modelFromFile = + createNoiseModel(v, smart, noiseFormat, kernelFunctionType); + return BinaryMeasurement(id1, id2, pose, + model ? model : modelFromFile); + } +}; + +/* ************************************************************************* */ +// Create a sampler to corrupt a measurement boost::shared_ptr createSampler(const SharedNoiseModel &model) { auto noise = boost::dynamic_pointer_cast(model); if (!noise) @@ -347,81 +374,75 @@ boost::shared_ptr createSampler(const SharedNoiseModel &model) { } /* ************************************************************************* */ -// Small functor to process the edge into a BetweenFactor::shared_ptr -struct ProcessPose2 { - // The arguments - Key maxKey; - SharedNoiseModel model; - boost::shared_ptr sampler; - - // Arguments for parsing noise model - bool smart; - NoiseFormat noiseFormat; - KernelFunctionType kernelFunctionType; - - ProcessPose2(Key maxKey = 0, SharedNoiseModel model = nullptr, - boost::shared_ptr sampler = nullptr, bool smart = true, - NoiseFormat noiseFormat = NoiseFormatAUTO, - KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE) - : maxKey(maxKey), model(model), sampler(sampler), smart(smart), - noiseFormat(noiseFormat), kernelFunctionType(kernelFunctionType) {} - - // The actual function - BetweenFactor::shared_ptr operator()(const Measurement2 &edge) { - // optional filter - if (maxKey && (edge.id1 >= maxKey || edge.id2 >= maxKey)) - return nullptr; - - // Get pose and optionally add noise - Pose2 T12 = edge.pose; - if (sampler) - T12 = T12.retract(sampler->sample()); - - // Create factor - // If model is nullptr we use the model from the file - return boost::make_shared>( - edge.id1, edge.id2, T12, - model - ? model - : createNoiseModel(edge.v, smart, noiseFormat, kernelFunctionType)); - } -}; - -/* ************************************************************************* */ -BetweenFactorPose2s -parse2DFactors(const string &filename, - const noiseModel::Diagonal::shared_ptr &corruptingNoise, - Key maxKey) { - ProcessPose2 process(maxKey, nullptr, - corruptingNoise ? createSampler(corruptingNoise) - : nullptr); - - return parseToVector::shared_ptr>(filename, - process); +// Implementation of parseMeasurements for Pose2 +template <> +std::vector> +parseMeasurements(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + ParseMeasurement parse{model ? createSampler(model) : nullptr, maxKey, + true, NoiseFormatAUTO, KernelFunctionTypeNONE}; + return parseToVector>(filename, parse); } /* ************************************************************************* */ -static void parseOthers(const string &filename, Key maxKey, - NonlinearFactorGraph::shared_ptr graph, - Values::shared_ptr initial) { - // Do a pass to get other types of measurements - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("load2D: can not find file " + filename); +// Implementation of parseMeasurements for Rot2, which converts from Pose2 - Key id1, id2; - while (!is.eof()) { - string tag; - if (!(is >> tag)) - break; +// Extract Rot2 measurement from Pose2 measurement +static BinaryMeasurement convert(const BinaryMeasurement &p) { + auto gaussian = + boost::dynamic_pointer_cast(p.noiseModel()); + if (!gaussian) + throw std::invalid_argument( + "parseMeasurements can only convert Pose2 measurements " + "with Gaussian noise models."); + const Matrix3 M = gaussian->covariance(); + return BinaryMeasurement(p.key1(), p.key2(), p.measured().rotation(), + noiseModel::Isotropic::Variance(1, M(2, 2))); +} - // Parse measurements - bool haveLandmark = false; +template <> +std::vector> +parseMeasurements(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + auto poses = parseMeasurements(filename, model, maxKey); + std::vector> result; + result.reserve(poses.size()); + for (const auto &p : poses) + result.push_back(convert(p)); + return result; +} + +/* ************************************************************************* */ +// Implementation of parseFactors for Pose2 +template <> +std::vector::shared_ptr> +parseFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + ParseFactor parse({model ? createSampler(model) : nullptr, maxKey, + true, NoiseFormatAUTO, KernelFunctionTypeNONE, + nullptr}); + return parseToVector::shared_ptr>(filename, parse); +} + +/* ************************************************************************* */ +using BearingRange2D = BearingRange; + +template <> struct ParseMeasurement { + // arguments + Key maxKey; + + // The actual parser + boost::optional> + operator()(istream &is, const string &tag) { + if (tag != "BR" && tag != "LANDMARK") + return boost::none; + + Key id1, id2; + is >> id1 >> id2; double bearing, range, bearing_std, range_std; - // A bearing-range measurement if (tag == "BR") { - is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; + is >> bearing >> range >> bearing_std >> range_std; } // A landmark measurement, converted to bearing-range @@ -429,7 +450,7 @@ static void parseOthers(const string &filename, Key maxKey, double lmx, lmy; double v1, v2, v3; - is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; + is >> lmx >> lmy >> v1 >> v2 >> v3; // Convert x,y to bearing,range bearing = atan2(lmy, lmx); @@ -442,90 +463,93 @@ static void parseOthers(const string &filename, Key maxKey, bearing_std = sqrt(v1 / 10.0); range_std = sqrt(v1); } else { + // TODO(frank): we are ignoring the non-uniform covariance bearing_std = 1; range_std = 1; - if (!haveLandmark) { - cout << "Warning: load2D is a very simple dataset loader and is " - "ignoring the\n" - "non-uniform covariance on LANDMARK measurements in this " - "file." - << endl; - haveLandmark = true; - } } } - // Do some common stuff for bearing-range measurements - if (tag == "LANDMARK" || tag == "BR") { + // optional filter + if (maxKey && id1 >= maxKey) + return boost::none; - // optional filter - if (maxKey && id1 >= maxKey) - continue; + // Create noise model + auto measurementNoise = noiseModel::Diagonal::Sigmas( + (Vector(2) << bearing_std, range_std).finished()); - // Create noise model - noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas( - (Vector(2) << bearing_std, range_std).finished()); - - // Add to graph - *graph += BearingRangeFactor(id1, L(id2), bearing, range, - measurementNoise); - - // Insert poses or points if they do not exist yet - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(L(id2))) { - Pose2 pose = initial->at(id1); - Point2 local(cos(bearing) * range, sin(bearing) * range); - Point2 global = pose.transformFrom(local); - initial->insert(L(id2), global); - } - } - is.ignore(LINESIZE, '\n'); + return BinaryMeasurement( + id1, L(id2), BearingRange2D(bearing, range), measurementNoise); } -} +}; /* ************************************************************************* */ -GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxKey, - bool addNoise, bool smart, NoiseFormat noiseFormat, +GraphAndValues load2D(const string &filename, SharedNoiseModel model, + Key maxKey, bool addNoise, bool smart, + NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - Values::shared_ptr initial(new Values); + auto graph = boost::make_shared(); + auto initial = boost::make_shared(); - const auto poses = parse2DPoses(filename, maxKey); + // TODO(frank): do a single pass + const auto poses = parseVariables(filename, maxKey); for (const auto &key_pose : poses) { initial->insert(key_pose.first, key_pose.second); } - const auto landmarks = parse2DLandmarks(filename, maxKey); + + const auto landmarks = parseVariables(filename, maxKey); for (const auto &key_landmark : landmarks) { - initial->insert(key_landmark.first, key_landmark.second); + initial->insert(L(key_landmark.first), key_landmark.second); } - // Parse the pose constraints - ProcessPose2 process(maxKey, model, addNoise ? createSampler(model) : nullptr, - smart, noiseFormat, kernelFunctionType); - auto factors = parseToVector::shared_ptr>( - filename, process); + // Create parser for Pose2 and bearing/range + ParseFactor parseBetweenFactor( + {addNoise ? createSampler(model) : nullptr, maxKey, smart, noiseFormat, + kernelFunctionType, model}); + ParseMeasurement parseBearingRange{maxKey}; - // Add factors into the graph and add poses if necessary - auto graph = boost::make_shared(); - for (const auto &f : factors) { - graph->push_back(f); + Parser parse = [&](istream &is, const string &tag) { + if (auto f = parseBetweenFactor(is, tag)) { + graph->push_back(*f); - // Insert vertices if pure odometry file - Key id1 = f->key1(), id2 = f->key2(); - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) - initial->insert(id2, initial->at(id1) * f->measured()); - } + // Insert vertices if pure odometry file + Key key1 = (*f)->key1(), key2 = (*f)->key2(); + if (!initial->exists(key1)) + initial->insert(key1, Pose2()); + if (!initial->exists(key2)) + initial->insert(key2, initial->at(key1) * (*f)->measured()); + } else if (auto m = parseBearingRange(is, tag)) { + Key key1 = m->key1(), key2 = m->key2(); + BearingRange2D br = m->measured(); + graph->emplace_shared>(key1, key2, br, + m->noiseModel()); - // Do a pass to parse landmarks and bearing-range measurements - parseOthers(filename, maxKey, graph, initial); + // Insert poses or points if they do not exist yet + if (!initial->exists(key1)) + initial->insert(key1, Pose2()); + if (!initial->exists(key2)) { + Pose2 pose = initial->at(key1); + Point2 local = br.bearing() * Point2(br.range(), 0); + Point2 global = pose.transformFrom(local); + initial->insert(key2, global); + } + } + return 0; + }; + + parseLines(filename, parse); return make_pair(graph, initial); } +/* ************************************************************************* */ +GraphAndValues load2D(pair dataset, Key maxKey, + bool addNoise, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + return load2D(dataset.first, dataset.second, maxKey, addNoise, smart, + noiseFormat, kernelFunctionType); +} + /* ************************************************************************* */ GraphAndValues load2D_robust(const string &filename, noiseModel::Base::shared_ptr &model, Key maxKey) { @@ -540,7 +564,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config, fstream stream(filename.c_str(), fstream::out); // save poses - for (const Values::ConstKeyValuePair &key_value : config) { + for (const Values::ConstKeyValuePair key_value : config) { const Pose2 &pose = key_value.value.cast(); stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; @@ -586,7 +610,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, fstream stream(filename.c_str(), fstream::out); // save 2D poses - for (const auto &key_value : estimate) { + for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; @@ -596,7 +620,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, } // save 3D poses - for (const auto &key_value : estimate) { + for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; @@ -609,7 +633,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, } // save 2D landmarks - for (const auto &key_value : estimate) { + for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; @@ -619,7 +643,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, } // save 3D landmarks - for (const auto &key_value : estimate) { + for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; @@ -709,44 +733,46 @@ istream &operator>>(istream &is, Rot3 &R) { } /* ************************************************************************* */ -istream &operator>>(istream &is, boost::optional> &indexed) { - string tag; - is >> tag; +boost::optional> parseVertexPose3(istream &is, + const string &tag) { if (tag == "VERTEX3") { Key id; double x, y, z; Rot3 R; is >> id >> x >> y >> z >> R; - indexed.reset({id, Pose3(R, {x, y, z})}); + return make_pair(id, Pose3(R, {x, y, z})); } else if (tag == "VERTEX_SE3:QUAT") { Key id; double x, y, z; Quaternion q; is >> id >> x >> y >> z >> q; - indexed.reset({id, Pose3(q, {x, y, z})}); - } - return is; + return make_pair(id, Pose3(q, {x, y, z})); + } else + return boost::none; } -map parse3DPoses(const string &filename, Key maxKey) { - return parseIntoMap(filename, maxKey); +template <> +std::map parseVariables(const std::string &filename, + Key maxKey) { + return parseToMap(filename, parseVertexPose3, maxKey); } /* ************************************************************************* */ -istream &operator>>(istream &is, boost::optional> &indexed) { - string tag; - is >> tag; +boost::optional> parseVertexPoint3(istream &is, + const string &tag) { if (tag == "VERTEX_TRACKXYZ") { Key id; double x, y, z; is >> id >> x >> y >> z; - indexed.reset({id, Point3(x, y, z)}); - } - return is; + return make_pair(id, Point3(x, y, z)); + } else + return boost::none; } -map parse3DLandmarks(const string &filename, Key maxKey) { - return parseIntoMap(filename, maxKey); +template <> +std::map parseVariables(const std::string &filename, + Key maxKey) { + return parseToMap(filename, parseVertexPoint3, maxKey); } /* ************************************************************************* */ @@ -761,97 +787,126 @@ istream &operator>>(istream &is, Matrix6 &m) { } /* ************************************************************************* */ -struct Measurement3 { - Key id1, id2; - Pose3 pose; - SharedNoiseModel model; +// Pose3 measurement parser +template <> struct ParseMeasurement { + // The arguments + boost::shared_ptr sampler; + Key maxKey; + + // The actual parser + boost::optional> operator()(istream &is, + const string &tag) { + if (tag != "EDGE3" && tag != "EDGE_SE3:QUAT") + return boost::none; + + // parse ids and optionally filter + Key id1, id2; + is >> id1 >> id2; + if (maxKey && (id1 >= maxKey || id2 >= maxKey)) + return boost::none; + + Matrix6 m; + if (tag == "EDGE3") { + double x, y, z; + Rot3 R; + is >> x >> y >> z >> R >> m; + Pose3 T12(R, {x, y, z}); + // optionally add noise + if (sampler) + T12 = T12.retract(sampler->sample()); + + return BinaryMeasurement(id1, id2, T12, + noiseModel::Gaussian::Information(m)); + } else if (tag == "EDGE_SE3:QUAT") { + double x, y, z; + Quaternion q; + is >> x >> y >> z >> q >> m; + + Pose3 T12(q, {x, y, z}); + // optionally add noise + if (sampler) + T12 = T12.retract(sampler->sample()); + + // EDGE_SE3:QUAT stores covariance in t,R order, unlike GTSAM: + Matrix6 mgtsam; + mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation + mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal + SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); + + return BinaryMeasurement( + id1, id2, T12, noiseModel::Gaussian::Information(mgtsam)); + } else + return boost::none; + } }; -istream &operator>>(istream &is, boost::optional &edge) { - string tag; - is >> tag; - Matrix6 m; - if (tag == "EDGE3") { - Key id1, id2; - double x, y, z; - Rot3 R; - is >> id1 >> id2 >> x >> y >> z >> R >> m; - edge.reset( - {id1, id2, Pose3(R, {x, y, z}), noiseModel::Gaussian::Information(m)}); - } - if (tag == "EDGE_SE3:QUAT") { - Key id1, id2; - double x, y, z; - Quaternion q; - is >> id1 >> id2 >> x >> y >> z >> q >> m; - - // EDGE_SE3:QUAT stores covariance in t,R order, unlike GTSAM: - Matrix6 mgtsam; - mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation - mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation - mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal - mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal - SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - - edge.reset({id1, id2, Pose3(q, {x, y, z}), - noiseModel::Gaussian::Information(mgtsam)}); - } - return is; +/* ************************************************************************* */ +// Implementation of parseMeasurements for Pose3 +template <> +std::vector> +parseMeasurements(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + ParseMeasurement parse{model ? createSampler(model) : nullptr, maxKey}; + return parseToVector>(filename, parse); } /* ************************************************************************* */ -// Small functor to process the edge into a BetweenFactor::shared_ptr -struct ProcessPose3 { - // The arguments - Key maxKey = 0; - SharedNoiseModel model; - boost::shared_ptr sampler; +// Implementation of parseMeasurements for Rot3, which converts from Pose3 - // The actual function - BetweenFactor::shared_ptr operator()(const Measurement3 &edge) { - // optional filter - if (maxKey && (edge.id1 >= maxKey || edge.id2 >= maxKey)) - return nullptr; +// Extract Rot3 measurement from Pose3 measurement +static BinaryMeasurement convert(const BinaryMeasurement &p) { + auto gaussian = + boost::dynamic_pointer_cast(p.noiseModel()); + if (!gaussian) + throw std::invalid_argument( + "parseMeasurements can only convert Pose3 measurements " + "with Gaussian noise models."); + const Matrix6 M = gaussian->covariance(); + return BinaryMeasurement( + p.key1(), p.key2(), p.measured().rotation(), + noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); +} - // Get pose and optionally add noise - Pose3 T12 = edge.pose; - if (sampler) - T12 = T12.retract(sampler->sample()); - - // Create factor - return boost::make_shared>(edge.id1, edge.id2, T12, - edge.model); - } -}; +template <> +std::vector> +parseMeasurements(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + auto poses = parseMeasurements(filename, model, maxKey); + std::vector> result; + result.reserve(poses.size()); + for (const auto &p : poses) + result.push_back(convert(p)); + return result; +} /* ************************************************************************* */ -BetweenFactorPose3s -parse3DFactors(const string &filename, - const noiseModel::Diagonal::shared_ptr &corruptingNoise, - Key maxKey) { - ProcessPose3 process; - process.maxKey = maxKey; - if (corruptingNoise) { - process.sampler = createSampler(corruptingNoise); - } - return parseToVector::shared_ptr>(filename, - process); +// Implementation of parseFactors for Pose3 +template <> +std::vector::shared_ptr> +parseFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + ParseFactor parse({model ? createSampler(model) : nullptr, maxKey}); + return parseToVector::shared_ptr>(filename, parse); } /* ************************************************************************* */ GraphAndValues load3D(const string &filename) { - const auto factors = parse3DFactors(filename); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + auto graph = boost::make_shared(); + auto initial = boost::make_shared(); + + // TODO(frank): do a single pass + const auto factors = parseFactors(filename); for (const auto &factor : factors) { graph->push_back(factor); } - Values::shared_ptr initial(new Values); - - const auto poses = parse3DPoses(filename); + const auto poses = parseVariables(filename); for (const auto &key_pose : poses) { initial->insert(key_pose.first, key_pose.second); } + const auto landmarks = parse3DLandmarks(filename); for (const auto &key_landmark : landmarks) { initial->insert(key_landmark.first, key_landmark.second); @@ -1200,4 +1255,20 @@ Values initialCamerasAndPointsEstimate(const SfmData &db) { return initial; } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +std::map parse3DPoses(const std::string &filename, Key maxKey) { + return parseVariables(filename, maxKey); +} + +std::map parse3DLandmarks(const std::string &filename, + Key maxKey) { + return parseVariables(filename, maxKey); +} + +BetweenFactorPose3s +parse3DFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { + return parseFactors(filename, model, maxKey); +} +#endif } // namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 61eb9256e..38ddfd1dc 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include #include @@ -72,6 +73,34 @@ enum KernelFunctionType { KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY }; +/** + * Parse variables in 2D g2o graph file into a map. + * Instantiated in .cpp T equal to Pose2, Pose3 + */ +template +GTSAM_EXPORT std::map parseVariables(const std::string &filename, + Key maxKey = 0); + +/** + * Parse edges in 2D g2o graph file into a set of binary measuremnts. + * Instantiated in .cpp for T equal to Pose2, Pose3 + */ +template +GTSAM_EXPORT std::vector> +parseMeasurements(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model = nullptr, + Key maxKey = 0); + +/** + * Parse edges in 2D g2o graph file into a set of BetweenFactors. + * Instantiated in .cpp T equal to Pose2, Pose3 + */ +template +GTSAM_EXPORT std::vector::shared_ptr> +parseFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model = nullptr, + Key maxKey = 0); + /// Return type for auxiliary functions typedef std::pair IndexedPose; typedef std::pair IndexedLandmark; @@ -90,7 +119,6 @@ GTSAM_EXPORT boost::optional parseVertexPose(std::istream& is, * @param is input stream * @param tag string parsed from input stream, will only parse if vertex type */ - GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& is, const std::string& tag); @@ -102,23 +130,6 @@ GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& GTSAM_EXPORT boost::optional parseEdge(std::istream& is, const std::string& tag); -using BetweenFactorPose2s = - std::vector::shared_ptr>; - -/// Parse edges in 2D g2o graph file into a set of BetweenFactors. -GTSAM_EXPORT BetweenFactorPose2s parse2DFactors( - const std::string &filename, - const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr, - Key maxKey = 0); - -/// Parse vertices in 2D g2o graph file into a map of Pose2s. -GTSAM_EXPORT std::map parse2DPoses(const std::string &filename, - Key maxKey = 0); - -/// Parse landmarks in 2D g2o graph file into a map of Point2s. -GTSAM_EXPORT std::map parse2DLandmarks(const string &filename, - Key maxKey = 0); - /// Return type for load functions using GraphAndValues = std::pair; @@ -183,21 +194,6 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); -/// Parse edges in 3D TORO graph file into a set of BetweenFactors. -using BetweenFactorPose3s = std::vector::shared_ptr>; -GTSAM_EXPORT BetweenFactorPose3s parse3DFactors( - const std::string &filename, - const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr, - Key maxKey = 0); - -/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s. -GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, - Key maxKey = 0); - -/// Parse landmarks in 3D g2o graph file into a map of Point3s. -GTSAM_EXPORT std::map parse3DLandmarks(const std::string &filename, - Key maxKey = 0); - /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); @@ -335,14 +331,21 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 -/** - * Parse TORO/G2O vertex "id x y yaw" - * @param is input stream - * @param tag string parsed from input stream, will only parse if vertex type - */ inline boost::optional parseVertex(std::istream &is, const std::string &tag) { return parseVertexPose(is, tag); } + +GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, + Key maxKey = 0); + +GTSAM_EXPORT std::map parse3DLandmarks(const std::string &filename, + Key maxKey = 0); + +using BetweenFactorPose3s = std::vector::shared_ptr>; +GTSAM_EXPORT BetweenFactorPose3s parse3DFactors( + const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model = nullptr, Key maxKey = 0); + #endif } // namespace gtsam diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index bc65dc351..864f42162 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -104,8 +104,16 @@ TEST(dataSet, load2D) { boost::dynamic_pointer_cast>(graph->at(0)); EXPECT(assert_equal(expected, *actual)); - // Check factor parsing - const auto actualFactors = parse2DFactors(filename); + // Check binary measurements, Pose2 + auto measurements = parseMeasurements(filename, nullptr, 5); + EXPECT_LONGS_EQUAL(4, measurements.size()); + + // Check binary measurements, Rot2 + auto measurements2 = parseMeasurements(filename); + EXPECT_LONGS_EQUAL(300, measurements2.size()); + + // // Check factor parsing + const auto actualFactors = parseFactors(filename); for (size_t i : {0, 1, 2, 3, 4, 5}) { EXPECT(assert_equal( *boost::dynamic_pointer_cast>(graph->at(i)), @@ -113,13 +121,13 @@ TEST(dataSet, load2D) { } // Check pose parsing - const auto actualPoses = parse2DPoses(filename); + const auto actualPoses = parseVariables(filename); for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(initial->at(j), actualPoses.at(j), 1e-5)); } // Check landmark parsing - const auto actualLandmarks = parse2DLandmarks(filename); + const auto actualLandmarks = parseVariables(filename); EXPECT_LONGS_EQUAL(0, actualLandmarks.size()); } @@ -202,7 +210,7 @@ TEST(dataSet, readG2o3D) { } // Check factor parsing - const auto actualFactors = parse3DFactors(g2oFile); + const auto actualFactors = parseFactors(g2oFile); for (size_t i : {0, 1, 2, 3, 4, 5}) { EXPECT(assert_equal( *boost::dynamic_pointer_cast>(expectedGraph[i]), @@ -210,7 +218,7 @@ TEST(dataSet, readG2o3D) { } // Check pose parsing - const auto actualPoses = parse3DPoses(g2oFile); + const auto actualPoses = parseVariables(g2oFile); for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); } @@ -277,7 +285,7 @@ TEST(dataSet, readG2oCheckDeterminants) { const string g2oFile = findExampleDataFile("toyExample.g2o"); // Check determinants in factors - auto factors = parse3DFactors(g2oFile); + auto factors = parseFactors(g2oFile); EXPECT_LONGS_EQUAL(6, factors.size()); for (const auto& factor : factors) { const Rot3 R = factor->measured().rotation(); @@ -285,7 +293,7 @@ TEST(dataSet, readG2oCheckDeterminants) { } // Check determinants in initial values - const map poses = parse3DPoses(g2oFile); + const map poses = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(5, poses.size()); for (const auto& key_value : poses) { const Rot3 R = key_value.second.rotation(); @@ -300,7 +308,7 @@ TEST(dataSet, readG2oLandmarks) { const string g2oFile = findExampleDataFile("example_with_vertices.g2o"); // Check number of poses and landmarks. Should be 8 each. - const map poses = parse3DPoses(g2oFile); + const map poses = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(8, poses.size()); const map landmarks = parse3DLandmarks(g2oFile); EXPECT_LONGS_EQUAL(8, landmarks.size()); diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp index 8bdda968f..44bb084cc 100644 --- a/timing/timeFrobeniusFactor.cpp +++ b/timing/timeFrobeniusFactor.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) { // Read G2O file const auto factors = parse3DFactors(g2oFile); - const auto poses = parse3DPoses(g2oFile); + const auto poses = parseVariables(g2oFile); // Build graph NonlinearFactorGraph graph; From de7b56a491fde5ec3ed1536b4b8270787e97b2a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 14 Aug 2020 22:02:17 -0400 Subject: [PATCH 05/10] Turn off gcc build as it times out every time --- .travis.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 986e86cc2..d6c6cb156 100644 --- a/.travis.yml +++ b/.travis.yml @@ -112,11 +112,11 @@ jobs: compiler: gcc env: CMAKE_BUILD_TYPE=Release script: bash .travis.sh -t - - stage: test - os: linux - compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF - script: bash .travis.sh -t + # - stage: test + # os: linux + # compiler: gcc + # env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + # script: bash .travis.sh -t - stage: test os: linux compiler: clang From 97537f2a36b85f62c46dc04bcd4ed23ec25c8b98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Aug 2020 08:06:15 -0400 Subject: [PATCH 06/10] Avoid clang warnings about double-brace initialization --- gtsam/sam/BearingRangeFactor.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index b8e231915..482dbb974 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -48,15 +48,15 @@ class BearingRangeFactor /// Construct from BearingRange instance BearingRangeFactor(Key key1, Key key2, const T &bearingRange, const SharedNoiseModel &model) - : Base({key1, key2}, model, T(bearingRange)) { - this->initialize(expression({key1, key2})); + : Base({{key1, key2}}, model, T(bearingRange)) { + this->initialize(expression({{key1, key2}})); } /// Construct from separate bearing and range BearingRangeFactor(Key key1, Key key2, const B &measuredBearing, const R &measuredRange, const SharedNoiseModel &model) - : Base({key1, key2}, model, T(measuredBearing, measuredRange)) { - this->initialize(expression({key1, key2})); + : Base({{key1, key2}}, model, T(measuredBearing, measuredRange)) { + this->initialize(expression({{key1, key2}})); } virtual ~BearingRangeFactor() {} From fc085626416b34c94379b9d76669a04746c87f56 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Aug 2020 08:06:37 -0400 Subject: [PATCH 07/10] Hunted down deprecated use of parse3DLandmarks --- gtsam/slam/dataset.cpp | 2 +- gtsam/slam/tests/testDataset.cpp | 6 +++--- timing/timeFrobeniusFactor.cpp | 12 ++++++------ 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 11473d084..2aac939d9 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -907,7 +907,7 @@ GraphAndValues load3D(const string &filename) { initial->insert(key_pose.first, key_pose.second); } - const auto landmarks = parse3DLandmarks(filename); + const auto landmarks = parseVariables(filename); for (const auto &key_landmark : landmarks) { initial->insert(key_landmark.first, key_landmark.second); } diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 864f42162..3e804b6b0 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -224,7 +224,7 @@ TEST(dataSet, readG2o3D) { } // Check landmark parsing - const auto actualLandmarks = parse3DLandmarks(g2oFile); + const auto actualLandmarks = parseVariables(g2oFile); for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); } @@ -299,7 +299,7 @@ TEST(dataSet, readG2oCheckDeterminants) { const Rot3 R = key_value.second.rotation(); EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9); } - const map landmarks = parse3DLandmarks(g2oFile); + const map landmarks = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(0, landmarks.size()); } @@ -310,7 +310,7 @@ TEST(dataSet, readG2oLandmarks) { // Check number of poses and landmarks. Should be 8 each. const map poses = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(8, poses.size()); - const map landmarks = parse3DLandmarks(g2oFile); + const map landmarks = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(8, landmarks.size()); } diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp index 44bb084cc..c0ac28ed9 100644 --- a/timing/timeFrobeniusFactor.cpp +++ b/timing/timeFrobeniusFactor.cpp @@ -57,7 +57,7 @@ int main(int argc, char* argv[]) { } // Read G2O file - const auto factors = parse3DFactors(g2oFile); + const auto measurements = parseMeasurements(g2oFile); const auto poses = parseVariables(g2oFile); // Build graph @@ -66,12 +66,12 @@ int main(int argc, char* argv[]) { auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); graph.add(PriorFactor(0, SOn::identity(4), priorModel)); auto G = boost::make_shared(SOn::VectorizedGenerators(4)); - for (const auto& factor : factors) { - const auto& keys = factor->keys(); - const auto& Tij = factor->measured(); - const auto& model = factor->noiseModel(); + for (const auto &m : measurements) { + const auto &keys = m.keys(); + const Rot3 &Tij = m.measured(); + const auto &model = m.noiseModel(); graph.emplace_shared( - keys[0], keys[1], Rot3(Tij.rotation().matrix()), 4, model, G); + keys[0], keys[1], Tij, 4, model, G); } std::mt19937 rng(42); From b1e3b5495ce5c969d76f9363b6bb0828b9f250c1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Aug 2020 13:05:58 -0400 Subject: [PATCH 08/10] Some behavior changes. - maxIndex now does what it says - id = size_t, Key is only for Values and Graph) - re-admitted methods needed for wrapper. --- examples/Data/example_with_vertices.g2o | 32 +-- gtsam/slam/dataset.cpp | 268 +++++++++++++----------- gtsam/slam/dataset.h | 71 ++++--- gtsam/slam/tests/testDataset.cpp | 46 ++-- tests/testNonlinearISAM.cpp | 2 +- 5 files changed, 229 insertions(+), 190 deletions(-) diff --git a/examples/Data/example_with_vertices.g2o b/examples/Data/example_with_vertices.g2o index 6c2f1a530..ca7cd86df 100644 --- a/examples/Data/example_with_vertices.g2o +++ b/examples/Data/example_with_vertices.g2o @@ -1,16 +1,16 @@ -VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162 -VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508 -VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10 -VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508 -VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162 -VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567 -VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412 -VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567 -VERTEX_TRACKXYZ 7782220156096217088 10 10 10 -VERTEX_TRACKXYZ 7782220156096217089 -10 10 10 -VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10 -VERTEX_TRACKXYZ 7782220156096217091 10 -10 10 -VERTEX_TRACKXYZ 7782220156096217092 10 10 -10 -VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10 -VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10 -VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10 +VERTEX_SE3:QUAT 0 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162 +VERTEX_SE3:QUAT 1 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508 +VERTEX_SE3:QUAT 2 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10 +VERTEX_SE3:QUAT 3 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508 +VERTEX_SE3:QUAT 4 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162 +VERTEX_SE3:QUAT 5 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567 +VERTEX_SE3:QUAT 6 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412 +VERTEX_SE3:QUAT 7 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567 +VERTEX_TRACKXYZ 0 10 10 10 +VERTEX_TRACKXYZ 1 -10 10 10 +VERTEX_TRACKXYZ 2 -10 -10 10 +VERTEX_TRACKXYZ 3 10 -10 10 +VERTEX_TRACKXYZ 4 10 10 -10 +VERTEX_TRACKXYZ 5 -10 10 -10 +VERTEX_TRACKXYZ 6 -10 -10 -10 +VERTEX_TRACKXYZ 7 10 -10 -10 diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 2aac939d9..9cc814245 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -53,7 +53,9 @@ using namespace std; namespace fs = boost::filesystem; -using namespace gtsam::symbol_shorthand; +using gtsam::symbol_shorthand::L; +using gtsam::symbol_shorthand::P; +using gtsam::symbol_shorthand::X; #define LINESIZE 81920 @@ -127,19 +129,16 @@ static void parseLines(const string &filename, Parser parse) { } /* ************************************************************************* */ -// Parse types T into a Key-indexed map +// Parse types T into a size_t-indexed map template -map parseToMap(const string &filename, Parser> parse, - Key maxKey) { - ifstream is(filename.c_str()); - if (!is) - throw invalid_argument("parse: can not find file " + filename); - - map result; - Parser> emplace = [&result, parse](istream &is, - const string &tag) { - if (auto t = parse(is, tag)) - result.emplace(*t); +map parseToMap(const string &filename, Parser> parse, + size_t maxIndex) { + map result; + Parser> emplace = [&](istream &is, const string &tag) { + if (auto t = parse(is, tag)) { + if (!maxIndex || t->first <= maxIndex) + result.emplace(*t); + } return boost::none; }; parseLines(filename, emplace); @@ -148,7 +147,6 @@ map parseToMap(const string &filename, Parser> parse, /* ************************************************************************* */ // Parse a file and push results on a vector -// TODO(frank): do we need this *and* parseMeasurements? template static vector parseToVector(const string &filename, Parser parse) { vector result; @@ -164,7 +162,7 @@ static vector parseToVector(const string &filename, Parser parse) { /* ************************************************************************* */ boost::optional parseVertexPose(istream &is, const string &tag) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { - Key id; + size_t id; double x, y, yaw; is >> id >> x >> y >> yaw; return IndexedPose(id, Pose2(x, y, yaw)); @@ -174,16 +172,16 @@ boost::optional parseVertexPose(istream &is, const string &tag) { } template <> -std::map parseVariables(const std::string &filename, - Key maxKey) { - return parseToMap(filename, parseVertexPose, maxKey); +std::map parseVariables(const std::string &filename, + size_t maxIndex) { + return parseToMap(filename, parseVertexPose, maxIndex); } /* ************************************************************************* */ boost::optional parseVertexLandmark(istream &is, const string &tag) { if (tag == "VERTEX_XY") { - Key id; + size_t id; double x, y; is >> id >> x >> y; return IndexedLandmark(id, Point2(x, y)); @@ -193,9 +191,9 @@ boost::optional parseVertexLandmark(istream &is, } template <> -std::map parseVariables(const std::string &filename, - Key maxKey) { - return parseToMap(filename, parseVertexLandmark, maxKey); +std::map parseVariables(const std::string &filename, + size_t maxIndex) { + return parseToMap(filename, parseVertexLandmark, maxIndex); } /* ************************************************************************* */ @@ -287,10 +285,10 @@ boost::optional parseEdge(istream &is, const string &tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || (tag == "ODOMETRY")) { - Key id1, id2; + size_t id1, id2; double x, y, yaw; is >> id1 >> id2 >> x >> y >> yaw; - return IndexedEdge(pair(id1, id2), Pose2(x, y, yaw)); + return IndexedEdge({id1, id2}, Pose2(x, y, yaw)); } else { return boost::none; } @@ -323,7 +321,7 @@ template struct ParseFactor : ParseMeasurement { template <> struct ParseMeasurement { // The arguments boost::shared_ptr sampler; - Key maxKey; + size_t maxIndex; // For noise model creation bool smart; @@ -345,9 +343,9 @@ template <> struct ParseMeasurement { is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); // optional filter - Key id1, id2; + size_t id1, id2; tie(id1, id2) = edge->first; - if (maxKey && (id1 >= maxKey || id2 >= maxKey)) + if (maxIndex && (id1 > maxIndex || id2 > maxIndex)) return boost::none; // Get pose and optionally add noise @@ -378,9 +376,11 @@ boost::shared_ptr createSampler(const SharedNoiseModel &model) { template <> std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - ParseMeasurement parse{model ? createSampler(model) : nullptr, maxKey, - true, NoiseFormatAUTO, KernelFunctionTypeNONE}; + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + ParseMeasurement parse{model ? createSampler(model) : nullptr, + maxIndex, true, NoiseFormatAUTO, + KernelFunctionTypeNONE}; return parseToVector>(filename, parse); } @@ -403,8 +403,9 @@ static BinaryMeasurement convert(const BinaryMeasurement &p) { template <> std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - auto poses = parseMeasurements(filename, model, maxKey); + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + auto poses = parseMeasurements(filename, model, maxIndex); std::vector> result; result.reserve(poses.size()); for (const auto &p : poses) @@ -417,8 +418,9 @@ parseMeasurements(const std::string &filename, template <> std::vector::shared_ptr> parseFactors(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - ParseFactor parse({model ? createSampler(model) : nullptr, maxKey, + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + ParseFactor parse({model ? createSampler(model) : nullptr, maxIndex, true, NoiseFormatAUTO, KernelFunctionTypeNONE, nullptr}); return parseToVector::shared_ptr>(filename, parse); @@ -429,7 +431,7 @@ using BearingRange2D = BearingRange; template <> struct ParseMeasurement { // arguments - Key maxKey; + size_t maxIndex; // The actual parser boost::optional> @@ -437,7 +439,7 @@ template <> struct ParseMeasurement { if (tag != "BR" && tag != "LANDMARK") return boost::none; - Key id1, id2; + size_t id1, id2; is >> id1 >> id2; double bearing, range, bearing_std, range_std; @@ -470,7 +472,7 @@ template <> struct ParseMeasurement { } // optional filter - if (maxKey && id1 >= maxKey) + if (maxIndex && id1 > maxIndex) return boost::none; // Create noise model @@ -484,30 +486,37 @@ template <> struct ParseMeasurement { /* ************************************************************************* */ GraphAndValues load2D(const string &filename, SharedNoiseModel model, - Key maxKey, bool addNoise, bool smart, + size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - auto graph = boost::make_shared(); + // Single pass for poses and landmarks. auto initial = boost::make_shared(); + Parser insert = [maxIndex, &initial](istream &is, const string &tag) { + if (auto indexedPose = parseVertexPose(is, tag)) { + if (!maxIndex || indexedPose->first <= maxIndex) + initial->insert(indexedPose->first, indexedPose->second); + } else if (auto indexedLandmark = parseVertexLandmark(is, tag)) { + if (!maxIndex || indexedLandmark->first <= maxIndex) + initial->insert(L(indexedLandmark->first), indexedLandmark->second); + } + return 0; + }; + parseLines(filename, insert); - // TODO(frank): do a single pass - const auto poses = parseVariables(filename, maxKey); - for (const auto &key_pose : poses) { - initial->insert(key_pose.first, key_pose.second); - } + // Single pass for Pose2 and bearing-range factors. + auto graph = boost::make_shared(); - const auto landmarks = parseVariables(filename, maxKey); - for (const auto &key_landmark : landmarks) { - initial->insert(L(key_landmark.first), key_landmark.second); - } - - // Create parser for Pose2 and bearing/range + // Instantiate factor parser ParseFactor parseBetweenFactor( - {addNoise ? createSampler(model) : nullptr, maxKey, smart, noiseFormat, + {addNoise ? createSampler(model) : nullptr, maxIndex, smart, noiseFormat, kernelFunctionType, model}); - ParseMeasurement parseBearingRange{maxKey}; + // Instantiate bearing-range parser + ParseMeasurement parseBearingRange{maxIndex}; + + // 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) { if (auto f = parseBetweenFactor(is, tag)) { graph->push_back(*f); @@ -537,23 +546,24 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, return 0; }; - parseLines(filename, parse); + parseLines(filename, parse); return make_pair(graph, initial); } /* ************************************************************************* */ -GraphAndValues load2D(pair dataset, Key maxKey, +GraphAndValues load2D(pair dataset, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { - return load2D(dataset.first, dataset.second, maxKey, addNoise, smart, + return load2D(dataset.first, dataset.second, maxIndex, addNoise, smart, noiseFormat, kernelFunctionType); } /* ************************************************************************* */ GraphAndValues load2D_robust(const string &filename, - noiseModel::Base::shared_ptr &model, Key maxKey) { - return load2D(filename, model, maxKey); + noiseModel::Base::shared_ptr &model, + size_t maxIndex) { + return load2D(filename, model, maxIndex); } /* ************************************************************************* */ @@ -596,10 +606,10 @@ GraphAndValues readG2o(const string &g2oFile, const bool is3D, return load3D(g2oFile); } else { // just call load2D - Key maxKey = 0; + size_t maxIndex = 0; bool addNoise = false; bool smart = true; - return load2D(g2oFile, SharedNoiseModel(), maxKey, addNoise, smart, + return load2D(g2oFile, SharedNoiseModel(), maxIndex, addNoise, smart, NoiseFormatG2O, kernelFunctionType); } } @@ -609,13 +619,16 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const string &filename) { fstream stream(filename.c_str(), fstream::out); + // Use a lambda here to more easily modify behavior in future. + auto index = [](gtsam::Key key) { return Symbol(key).index(); }; + // save 2D poses for (const auto key_value : estimate) { auto p = dynamic_cast *>(&key_value.value); if (!p) continue; const Pose2 &pose = p->value(); - stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " + stream << "VERTEX_SE2 " << index(key_value.key) << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } @@ -627,7 +640,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const Pose3 &pose = p->value(); const Point3 t = pose.translation(); const auto q = pose.rotation().toQuaternion(); - stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " + stream << "VERTEX_SE3:QUAT " << index(key_value.key) << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } @@ -638,7 +651,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, if (!p) continue; const Point2 &point = p->value(); - stream << "VERTEX_XY " << key_value.key << " " << point.x() << " " + stream << "VERTEX_XY " << index(key_value.key) << " " << point.x() << " " << point.y() << endl; } @@ -648,26 +661,25 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, if (!p) continue; const Point3 &point = p->value(); - stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " " - << point.y() << " " << point.z() << endl; + stream << "VERTEX_TRACKXYZ " << index(key_value.key) << " " << point.x() + << " " << point.y() << " " << point.z() << endl; } // save edges (2D or 3D) for (const auto &factor_ : graph) { - boost::shared_ptr> factor = - boost::dynamic_pointer_cast>(factor_); + auto factor = boost::dynamic_pointer_cast>(factor_); if (factor) { SharedNoiseModel model = factor->noiseModel(); - boost::shared_ptr gaussianModel = - boost::dynamic_pointer_cast(model); + auto gaussianModel = boost::dynamic_pointer_cast(model); if (!gaussianModel) { model->print("model\n"); throw invalid_argument("writeG2o: invalid noise model!"); } Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R(); Pose2 pose = factor->measured(); //.inverse(); - stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " - << pose.x() << " " << pose.y() << " " << pose.theta(); + stream << "EDGE_SE2 " << index(factor->key1()) << " " + << index(factor->key2()) << " " << pose.x() << " " << pose.y() + << " " << pose.theta(); for (size_t i = 0; i < 3; i++) { for (size_t j = i; j < 3; j++) { stream << " " << Info(i, j); @@ -676,8 +688,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, stream << endl; } - boost::shared_ptr> factor3D = - boost::dynamic_pointer_cast>(factor_); + auto factor3D = boost::dynamic_pointer_cast>(factor_); if (factor3D) { SharedNoiseModel model = factor3D->noiseModel(); @@ -692,9 +703,10 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const Pose3 pose3D = factor3D->measured(); const Point3 p = pose3D.translation(); const auto q = pose3D.rotation().toQuaternion(); - stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() - << " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x() - << " " << q.y() << " " << q.z() << " " << q.w(); + stream << "EDGE_SE3:QUAT " << index(factor3D->key1()) << " " + << index(factor3D->key2()) << " " << p.x() << " " << p.y() << " " + << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " + << q.w(); Matrix6 InfoG2o = I_6x6; InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation @@ -733,16 +745,16 @@ istream &operator>>(istream &is, Rot3 &R) { } /* ************************************************************************* */ -boost::optional> parseVertexPose3(istream &is, - const string &tag) { +boost::optional> parseVertexPose3(istream &is, + const string &tag) { if (tag == "VERTEX3") { - Key id; + size_t id; double x, y, z; Rot3 R; is >> id >> x >> y >> z >> R; return make_pair(id, Pose3(R, {x, y, z})); } else if (tag == "VERTEX_SE3:QUAT") { - Key id; + size_t id; double x, y, z; Quaternion q; is >> id >> x >> y >> z >> q; @@ -752,16 +764,16 @@ boost::optional> parseVertexPose3(istream &is, } template <> -std::map parseVariables(const std::string &filename, - Key maxKey) { - return parseToMap(filename, parseVertexPose3, maxKey); +std::map parseVariables(const std::string &filename, + size_t maxIndex) { + return parseToMap(filename, parseVertexPose3, maxIndex); } /* ************************************************************************* */ -boost::optional> parseVertexPoint3(istream &is, - const string &tag) { +boost::optional> parseVertexPoint3(istream &is, + const string &tag) { if (tag == "VERTEX_TRACKXYZ") { - Key id; + size_t id; double x, y, z; is >> id >> x >> y >> z; return make_pair(id, Point3(x, y, z)); @@ -770,9 +782,9 @@ boost::optional> parseVertexPoint3(istream &is, } template <> -std::map parseVariables(const std::string &filename, - Key maxKey) { - return parseToMap(filename, parseVertexPoint3, maxKey); +std::map parseVariables(const std::string &filename, + size_t maxIndex) { + return parseToMap(filename, parseVertexPoint3, maxIndex); } /* ************************************************************************* */ @@ -791,7 +803,7 @@ istream &operator>>(istream &is, Matrix6 &m) { template <> struct ParseMeasurement { // The arguments boost::shared_ptr sampler; - Key maxKey; + size_t maxIndex; // The actual parser boost::optional> operator()(istream &is, @@ -800,9 +812,9 @@ template <> struct ParseMeasurement { return boost::none; // parse ids and optionally filter - Key id1, id2; + size_t id1, id2; is >> id1 >> id2; - if (maxKey && (id1 >= maxKey || id2 >= maxKey)) + if (maxIndex && (id1 > maxIndex || id2 > maxIndex)) return boost::none; Matrix6 m; @@ -847,8 +859,10 @@ template <> struct ParseMeasurement { template <> std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - ParseMeasurement parse{model ? createSampler(model) : nullptr, maxKey}; + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + ParseMeasurement parse{model ? createSampler(model) : nullptr, + maxIndex}; return parseToVector>(filename, parse); } @@ -872,8 +886,9 @@ static BinaryMeasurement convert(const BinaryMeasurement &p) { template <> std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - auto poses = parseMeasurements(filename, model, maxKey); + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + auto poses = parseMeasurements(filename, model, maxIndex); std::vector> result; result.reserve(poses.size()); for (const auto &p : poses) @@ -886,8 +901,9 @@ parseMeasurements(const std::string &filename, template <> std::vector::shared_ptr> parseFactors(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - ParseFactor parse({model ? createSampler(model) : nullptr, maxKey}); + const noiseModel::Diagonal::shared_ptr &model, + size_t maxIndex) { + ParseFactor parse({model ? createSampler(model) : nullptr, maxIndex}); return parseToVector::shared_ptr>(filename, parse); } @@ -896,21 +912,22 @@ GraphAndValues load3D(const string &filename) { auto graph = boost::make_shared(); auto initial = boost::make_shared(); - // TODO(frank): do a single pass - const auto factors = parseFactors(filename); - for (const auto &factor : factors) { - graph->push_back(factor); - } + // Instantiate factor parser. maxIndex is always zero for load3D. + ParseFactor parseFactor({nullptr, 0}); - const auto poses = parseVariables(filename); - for (const auto &key_pose : poses) { - initial->insert(key_pose.first, key_pose.second); - } - - const auto landmarks = parseVariables(filename); - for (const auto &key_landmark : landmarks) { - initial->insert(key_landmark.first, key_landmark.second); - } + // 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) { + if (auto indexedPose = parseVertexPose3(is, tag)) { + initial->insert(indexedPose->first, indexedPose->second); + } else if (auto indexedLandmark = parseVertexPoint3(is, tag)) { + initial->insert(L(indexedLandmark->first), indexedLandmark->second); + } else if (auto factor = parseFactor(is, tag)) { + graph->push_back(*factor); + } + return 0; + }; + parseLines(filename, parse); return make_pair(graph, initial); } @@ -1187,8 +1204,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data, dataValues.number_cameras()) { // we only estimated camera poses for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera - Key poseKey = symbol('x', i); - Pose3 pose = values.at(poseKey); + Pose3 pose = values.at(X(i)); Cal3Bundler K = dataValues.cameras[i].calibration(); Camera camera(pose, K); dataValues.cameras[i] = camera; @@ -1255,20 +1271,22 @@ Values initialCamerasAndPointsEstimate(const SfmData &db) { return initial; } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 -std::map parse3DPoses(const std::string &filename, Key maxKey) { - return parseVariables(filename, maxKey); -} - -std::map parse3DLandmarks(const std::string &filename, - Key maxKey) { - return parseVariables(filename, maxKey); -} - +// To be deprecated when pybind wrapper is merged: BetweenFactorPose3s parse3DFactors(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { - return parseFactors(filename, model, maxKey); + const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) { + return parseFactors(filename, model, maxIndex); +} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +std::map parse3DPoses(const std::string &filename, + size_t maxIndex) { + return parseVariables(filename, maxIndex); +} + +std::map parse3DLandmarks(const std::string &filename, + size_t maxIndex) { + return parseVariables(filename, maxIndex); } #endif } // namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 38ddfd1dc..7bdf6728d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -74,37 +74,41 @@ enum KernelFunctionType { }; /** - * Parse variables in 2D g2o graph file into a map. - * Instantiated in .cpp T equal to Pose2, Pose3 + * Parse variables in a line-based text format (like g2o) into a map. + * Instantiated in .cpp Pose2, Point2, Pose3, and Point3. + * Note the map keys are integer indices, *not* gtsam::Keys. This is is + * different below where landmarks will use be L(index) symbols. */ template -GTSAM_EXPORT std::map parseVariables(const std::string &filename, - Key maxKey = 0); +GTSAM_EXPORT std::map parseVariables(const std::string &filename, + size_t maxIndex = 0); /** - * Parse edges in 2D g2o graph file into a set of binary measuremnts. - * Instantiated in .cpp for T equal to Pose2, Pose3 + * Parse binary measurements in a line-based text format (like g2o) into a + * vector. Instantiated in .cpp for Pose2, Rot2, Pose3, and Rot3. The rotation + * versions parse poses and extract only the rotation part, using the marginal + * covariance as noise model. */ template GTSAM_EXPORT std::vector> parseMeasurements(const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model = nullptr, - Key maxKey = 0); + const noiseModel::Diagonal::shared_ptr &model = nullptr, + size_t maxIndex = 0); /** - * Parse edges in 2D g2o graph file into a set of BetweenFactors. - * Instantiated in .cpp T equal to Pose2, Pose3 + * Parse BetweenFactors in a line-based text format (like g2o) into a vector of + * shared pointers. Instantiated in .cpp T equal to Pose2 and Pose3. */ template GTSAM_EXPORT std::vector::shared_ptr> parseFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model = nullptr, - Key maxKey = 0); + size_t maxIndex = 0); /// Return type for auxiliary functions -typedef std::pair IndexedPose; -typedef std::pair IndexedLandmark; -typedef std::pair, Pose2> IndexedEdge; +typedef std::pair IndexedPose; +typedef std::pair IndexedLandmark; +typedef std::pair, Pose2> IndexedEdge; /** * Parse TORO/G2O vertex "id x y yaw" @@ -130,19 +134,21 @@ GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& GTSAM_EXPORT boost::optional parseEdge(std::istream& is, const std::string& tag); -/// Return type for load functions +/// Return type for load functions, which return a graph and initial values. For +/// landmarks, the gtsam::Symbol L(index) is used to insert into the Values. +/// Bearing-range measurements also refer tio landmarks with L(index). using GraphAndValues = std::pair; /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] - * @param maxKey if non-zero cut out vertices >= maxKey + * @param maxIndex if non-zero cut out vertices >= maxIndex * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ GTSAM_EXPORT GraphAndValues load2D( - std::pair dataset, Key maxKey = 0, + std::pair dataset, size_t maxIndex = 0, bool addNoise = false, bool smart = true, // NoiseFormat noiseFormat = NoiseFormatAUTO, @@ -152,7 +158,7 @@ GTSAM_EXPORT GraphAndValues load2D( * Load TORO/G2O style graph files * @param filename * @param model optional noise model to use instead of one specified by file - * @param maxKey if non-zero cut out vertices >= maxKey + * @param maxIndex if non-zero cut out vertices >= maxIndex * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model * @param noiseFormat how noise parameters are stored @@ -160,13 +166,13 @@ GTSAM_EXPORT GraphAndValues load2D( * @return graph and initial values */ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, - SharedNoiseModel model = SharedNoiseModel(), Key maxKey = 0, bool addNoise = + SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise = false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, - noiseModel::Base::shared_ptr& model, Key maxKey = 0); + noiseModel::Base::shared_ptr& model, size_t maxIndex = 0); /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, @@ -190,6 +196,11 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D * @param filename The name of the g2o file to write * @param graph NonlinearFactor graph storing the measurements * @param estimate Values + * + * Note:behavior change in PR #471: to be consistent with load2D and load3D, we + * write the *indices* to file and not the full Keys. This change really only + * affects landmarks, which get read as indices but stored in values with the + * symbol L(index). */ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); @@ -330,22 +341,24 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); */ GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); +// To be deprecated when pybind wrapper is merged: +using BetweenFactorPose3s = std::vector::shared_ptr>; +GTSAM_EXPORT BetweenFactorPose3s +parse3DFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model = nullptr, + size_t maxIndex = 0); + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 inline boost::optional parseVertex(std::istream &is, const std::string &tag) { return parseVertexPose(is, tag); } -GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, - Key maxKey = 0); +GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, + size_t maxIndex = 0); -GTSAM_EXPORT std::map parse3DLandmarks(const std::string &filename, - Key maxKey = 0); - -using BetweenFactorPose3s = std::vector::shared_ptr>; -GTSAM_EXPORT BetweenFactorPose3s parse3DFactors( - const std::string &filename, - const noiseModel::Diagonal::shared_ptr &model = nullptr, Key maxKey = 0); +GTSAM_EXPORT std::map +parse3DLandmarks(const std::string &filename, size_t maxIndex = 0); #endif } // namespace gtsam diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 3e804b6b0..aad9124c5 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -82,7 +82,7 @@ TEST( dataSet, parseEdge) const auto actual = parseEdge(is, tag); EXPECT(actual); if (actual) { - pair expected(0, 1); + pair expected(0, 1); EXPECT(expected == actual->first); EXPECT(assert_equal(Pose2(2, 3, 4), actual->second)); } @@ -105,8 +105,9 @@ TEST(dataSet, load2D) { EXPECT(assert_equal(expected, *actual)); // Check binary measurements, Pose2 - auto measurements = parseMeasurements(filename, nullptr, 5); - EXPECT_LONGS_EQUAL(4, measurements.size()); + size_t maxIndex = 5; + auto measurements = parseMeasurements(filename, nullptr, maxIndex); + EXPECT_LONGS_EQUAL(5, measurements.size()); // Check binary measurements, Rot2 auto measurements2 = parseMeasurements(filename); @@ -132,14 +133,22 @@ TEST(dataSet, load2D) { } /* ************************************************************************* */ -TEST( dataSet, load2DVictoriaPark) -{ +TEST(dataSet, load2DVictoriaPark) { const string filename = findExampleDataFile("victoria_park.txt"); NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; + + // Load all boost::tie(graph, initial) = load2D(filename); - EXPECT_LONGS_EQUAL(10608,graph->size()); - EXPECT_LONGS_EQUAL(7120,initial->size()); + EXPECT_LONGS_EQUAL(10608, graph->size()); + EXPECT_LONGS_EQUAL(7120, initial->size()); + + // Restrict keys + size_t maxIndex = 5; + boost::tie(graph, initial) = load2D(filename, nullptr, maxIndex); + EXPECT_LONGS_EQUAL(5, graph->size()); + EXPECT_LONGS_EQUAL(6, initial->size()); // file has 0 as well + EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]); } /* ************************************************************************* */ @@ -293,13 +302,13 @@ TEST(dataSet, readG2oCheckDeterminants) { } // Check determinants in initial values - const map poses = parseVariables(g2oFile); + const map poses = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(5, poses.size()); for (const auto& key_value : poses) { const Rot3 R = key_value.second.rotation(); EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9); } - const map landmarks = parseVariables(g2oFile); + const map landmarks = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(0, landmarks.size()); } @@ -308,10 +317,13 @@ TEST(dataSet, readG2oLandmarks) { const string g2oFile = findExampleDataFile("example_with_vertices.g2o"); // Check number of poses and landmarks. Should be 8 each. - const map poses = parseVariables(g2oFile); + const map poses = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(8, poses.size()); - const map landmarks = parseVariables(g2oFile); + const map landmarks = parseVariables(g2oFile); EXPECT_LONGS_EQUAL(8, landmarks.size()); + + auto graphAndValues = load3D(g2oFile); + EXPECT(graphAndValues.second->exists(L(0))); } /* ************************************************************************* */ @@ -564,14 +576,12 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ Values value; for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera - Key poseKey = symbol('x',i); Pose3 pose = poseChange.compose(readData.cameras[i].pose()); - value.insert(poseKey, pose); + value.insert(X(i), pose); } for(size_t j=0; j < readData.number_tracks(); j++){ // for each point - Key pointKey = P(j); Point3 point = poseChange.transformFrom( readData.tracks[j].p ); - value.insert(pointKey, point); + value.insert(P(j), point); } // Write values and readData to a file @@ -596,13 +606,11 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); - Key poseKey = symbol('x',0); - Pose3 actualPose = value.at(poseKey); + Pose3 actualPose = value.at(X(0)); EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); Point3 expectedPoint = track0.p; - Key pointKey = P(0); - Point3 actualPoint = value.at(pointKey); + Point3 actualPoint = value.at(P(0)); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 974806612..4249f5bc4 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -308,7 +308,7 @@ TEST(testNonlinearISAM, loop_closures ) { // check if edge const auto betweenPose = parseEdge(is, tag); if (betweenPose) { - Key id1, id2; + size_t id1, id2; tie(id1, id2) = betweenPose->first; graph.emplace_shared >(Symbol('x', id2), Symbol('x', id1), betweenPose->second, model); From 4f28a0b88d39e53c8e1ed64e719dcc75a6b35f3c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Aug 2020 17:54:45 -0400 Subject: [PATCH 09/10] Fixed covariance bug --- gtsam/slam/dataset.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9cc814245..12147adf2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -880,7 +880,7 @@ static BinaryMeasurement convert(const BinaryMeasurement &p) { const Matrix6 M = gaussian->covariance(); return BinaryMeasurement( p.key1(), p.key2(), p.measured().rotation(), - noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); + noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0), true)); } template <> From 8288b55d4e10181e3c2eaf2328815a69167b5671 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Aug 2020 17:30:52 -0400 Subject: [PATCH 10/10] Addressed review comments --- gtsam/slam/dataset.cpp | 30 ++++++++++++++++++++---------- gtsam/slam/dataset.h | 4 ++-- timing/timeFrobeniusFactor.cpp | 4 ++-- 3 files changed, 24 insertions(+), 14 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 12147adf2..d7b067d70 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -110,19 +110,21 @@ string createRewrittenFileName(const string &name) { } /* ************************************************************************* */ -// Parse a file by calling the parse(is, tag) function for every line. +// Type for parser functions used in parseLines below. template using Parser = std::function(istream &is, const 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()); if (!is) throw invalid_argument("parse: can not find file " + filename); - while (!is.eof()) { - string tag; - is >> tag; + string tag; + while (is >> tag) { parse(is, tag); // ignore return value is.ignore(LINESIZE, '\n'); } @@ -164,7 +166,9 @@ boost::optional parseVertexPose(istream &is, const string &tag) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { size_t id; double x, y, yaw; - is >> id >> x >> y >> yaw; + if (!(is >> id >> x >> y >> yaw)) { + throw std::runtime_error("parseVertexPose encountered malformed line"); + } return IndexedPose(id, Pose2(x, y, yaw)); } else { return boost::none; @@ -183,7 +187,10 @@ boost::optional parseVertexLandmark(istream &is, if (tag == "VERTEX_XY") { size_t id; double x, y; - is >> id >> x >> y; + if (!(is >> id >> x >> y)) { + throw std::runtime_error( + "parseVertexLandmark encountered malformed line"); + } return IndexedLandmark(id, Point2(x, y)); } else { return boost::none; @@ -287,7 +294,9 @@ boost::optional parseEdge(istream &is, const string &tag) { size_t id1, id2; double x, y, yaw; - is >> id1 >> id2 >> x >> y >> yaw; + if (!(is >> id1 >> id2 >> x >> y >> yaw)) { + throw std::runtime_error("parseEdge encountered malformed line"); + } return IndexedEdge({id1, id2}, Pose2(x, y, yaw)); } else { return boost::none; @@ -295,8 +304,8 @@ boost::optional parseEdge(istream &is, const string &tag) { } /* ************************************************************************* */ -// Measurement parsers are implemented as a functor, as they has several options -// to filter and create the measurement model. +// Measurement parsers are implemented as a functor, as they have several +// options to filter and create the measurement model. template struct ParseMeasurement; /* ************************************************************************* */ @@ -670,7 +679,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, auto factor = boost::dynamic_pointer_cast>(factor_); if (factor) { SharedNoiseModel model = factor->noiseModel(); - auto gaussianModel = boost::dynamic_pointer_cast(model); + auto gaussianModel = + boost::dynamic_pointer_cast(model); if (!gaussianModel) { model->print("model\n"); throw invalid_argument("writeG2o: invalid noise model!"); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7bdf6728d..53abe55ba 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -77,7 +77,7 @@ enum KernelFunctionType { * Parse variables in a line-based text format (like g2o) into a map. * Instantiated in .cpp Pose2, Point2, Pose3, and Point3. * Note the map keys are integer indices, *not* gtsam::Keys. This is is - * different below where landmarks will use be L(index) symbols. + * different below where landmarks will use L(index) symbols. */ template GTSAM_EXPORT std::map parseVariables(const std::string &filename, @@ -136,7 +136,7 @@ GTSAM_EXPORT boost::optional parseEdge(std::istream& is, /// Return type for load functions, which return a graph and initial values. For /// landmarks, the gtsam::Symbol L(index) is used to insert into the Values. -/// Bearing-range measurements also refer tio landmarks with L(index). +/// Bearing-range measurements also refer to landmarks with L(index). using GraphAndValues = std::pair; diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp index c0ac28ed9..924213a33 100644 --- a/timing/timeFrobeniusFactor.cpp +++ b/timing/timeFrobeniusFactor.cpp @@ -68,10 +68,10 @@ int main(int argc, char* argv[]) { auto G = boost::make_shared(SOn::VectorizedGenerators(4)); for (const auto &m : measurements) { const auto &keys = m.keys(); - const Rot3 &Tij = m.measured(); + const Rot3 &Rij = m.measured(); const auto &model = m.noiseModel(); graph.emplace_shared( - keys[0], keys[1], Tij, 4, model, G); + keys[0], keys[1], Rij, 4, model, G); } std::mt19937 rng(42);