From 7d274315724268c858a269991e6cafafd6c13f1d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 11 Jul 2020 02:52:34 -0400 Subject: [PATCH 1/4] support for landmarks in g2o files --- gtsam/slam/dataset.cpp | 102 ++++++++++++++++++++++++++----- gtsam/slam/dataset.h | 17 +++++- gtsam/slam/tests/testDataset.cpp | 27 +++++++- 3 files changed, 128 insertions(+), 18 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 669935994..cb15f84a8 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -11,7 +11,10 @@ /** * @file dataset.cpp * @date Jan 22, 2010 - * @author Kai Ni, Luca Carlone, Frank Dellaert + * @author Kai Ni + * @author Luca Carlone + * @author Frank Dellaert + * @author Varun Agrawal * @brief utility functions for loading datasets */ @@ -193,7 +196,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, } /* ************************************************************************* */ -boost::optional parseVertex(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; @@ -204,6 +207,18 @@ boost::optional parseVertex(istream& is, const string& tag) { } } +/* ************************************************************************* */ +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; + } +} + /* ************************************************************************* */ boost::optional parseEdge(istream& is, const string& tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") @@ -232,12 +247,12 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, string tag; - // load the poses + // load the poses and landmarks while (!is.eof()) { if (!(is >> tag)) break; - const auto indexed_pose = parseVertex(is, tag); + const auto indexed_pose = parseVertexPose(is, tag); if (indexed_pose) { Key id = indexed_pose->first; @@ -247,6 +262,16 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, 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 */ @@ -429,7 +454,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const string& filename) { fstream stream(filename.c_str(), fstream::out); - // save 2D & 3D poses + // save 2D poses for (const auto& key_value : estimate) { auto p = dynamic_cast*>(&key_value.value); if (!p) continue; @@ -438,15 +463,34 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, << 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(); - 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; + 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; + } + + // save 2D landmarks + 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; + } + + // save 3D landmarks + 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; } // save edges (2D or 3D) @@ -515,6 +559,7 @@ static Rot3 NormalizedRot3(double w, double x, double y, double z) { 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); } + /* ************************************************************************* */ std::map parse3DPoses(const string& filename) { ifstream is(filename.c_str()); @@ -545,6 +590,30 @@ std::map parse3DPoses(const string& filename) { return poses; } +/* ************************************************************************* */ +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)); + } + } + return landmarks; +} + /* ************************************************************************* */ BetweenFactorPose3s parse3DFactors( const string& filename, @@ -617,11 +686,16 @@ GraphAndValues load3D(const string& filename) { graph->push_back(factor); } - const auto poses = parse3DPoses(filename); Values::shared_ptr initial(new Values); + + const auto poses = parse3DPoses(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); + } return make_pair(graph, initial); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 032799429..a18ae12f6 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -76,6 +76,7 @@ enum KernelFunctionType { /// Return type for auxiliary functions typedef std::pair IndexedPose; +typedef std::pair IndexedLandmark; typedef std::pair, Pose2> IndexedEdge; /** @@ -83,9 +84,18 @@ typedef std::pair, Pose2> IndexedEdge; * @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, +GTSAM_EXPORT boost::optional parseVertexPose(std::istream& is, const std::string& tag); +/** + * Parse G2O landmark vertex "id x y" + * @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) + /** * Parse TORO/G2O edge "id1 id2 x y yaw" * @param is input stream @@ -162,9 +172,12 @@ using BetweenFactorPose3s = std::vector::shared_ptr> GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr); -/// Parse vertices in 3D TORO graph file into a map of Pose3s. +/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s. GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); +/// Parse landmarks in 3D g2o graph file into a map of Point3s. +GTSAM_EXPORT std::map parse3DLandmarks(const string& filename) + /// 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 8088ab18a..136b7a93c 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -43,13 +43,13 @@ TEST(dataSet, findExampleDataFile) { } /* ************************************************************************* */ -TEST( dataSet, parseVertex) +TEST( dataSet, parseVertexPose) { const string str = "VERTEX2 1 2.000000 3.000000 4.000000"; istringstream is(str); string tag; EXPECT(is >> tag); - const auto actual = parseVertex(is, tag); + const auto actual = parseVertexPose(is, tag); EXPECT(actual); if (actual) { EXPECT_LONGS_EQUAL(1, actual->first); @@ -57,6 +57,21 @@ TEST( dataSet, parseVertex) } } +/* ************************************************************************* */ +TEST( dataSet, parseVertexLandmark) +{ + const string str = "VERTEX_XY 1 2.000000 3.000000"; + istringstream is(str); + string tag; + EXPECT(is >> tag); + const auto actual = parseVertexLandmark(is, tag); + EXPECT(actual); + if (actual) { + EXPECT_LONGS_EQUAL(1, actual->first); + EXPECT(assert_equal(Point2(2, 3), actual->second)); + } +} + /* ************************************************************************* */ TEST( dataSet, parseEdge) { @@ -182,6 +197,12 @@ TEST(dataSet, readG2o3D) { EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); } + // Check landmark parsing + const auto actualLandmarks = parse3DLandmarks(g2oFile); + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); + } + // Check graph version NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; @@ -252,6 +273,8 @@ 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); + EXPECT_LONGS_EQUAL(0, landmarks.size()); } /* ************************************************************************* */ From 05f50eae40842abed479164d39903be905396692 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 03:10:03 -0500 Subject: [PATCH 2/4] Fix minor bugs --- gtsam/slam/dataset.h | 4 ++-- tests/testNonlinearISAM.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a18ae12f6..439a69fdc 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -94,7 +94,7 @@ GTSAM_EXPORT boost::optional parseVertexPose(std::istream& is, */ GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& is, - const std::string& tag) + const std::string& tag); /** * Parse TORO/G2O edge "id1 id2 x y yaw" @@ -176,7 +176,7 @@ GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); /// 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 string& filename); /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 88ae508b6..974806612 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -288,7 +288,7 @@ TEST(testNonlinearISAM, loop_closures ) { break; // Check if vertex - const auto indexedPose = parseVertex(is, tag); + const auto indexedPose = parseVertexPose(is, tag); if (indexedPose) { Key id = indexedPose->first; initialEstimate.insert(Symbol('x', id), indexedPose->second); From 8a210188f3923fa8007168d0ba941fe450fdef9e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 03:10:14 -0500 Subject: [PATCH 3/4] test for readG2o --- examples/Data/example_with_vertices.g2o | 16 ++++++++++++++++ gtsam/slam/tests/testDataset.cpp | 11 +++++++++++ 2 files changed, 27 insertions(+) create mode 100644 examples/Data/example_with_vertices.g2o diff --git a/examples/Data/example_with_vertices.g2o b/examples/Data/example_with_vertices.g2o new file mode 100644 index 000000000..6c2f1a530 --- /dev/null +++ b/examples/Data/example_with_vertices.g2o @@ -0,0 +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 diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 136b7a93c..b6b1776d2 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -277,6 +277,17 @@ TEST(dataSet, readG2oCheckDeterminants) { EXPECT_LONGS_EQUAL(0, landmarks.size()); } +/* ************************************************************************* */ +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); + EXPECT_LONGS_EQUAL(8, poses.size()); + const map landmarks = parse3DLandmarks(g2oFile); + EXPECT_LONGS_EQUAL(8, landmarks.size()); +} + /* ************************************************************************* */ static NonlinearFactorGraph expectedGraph(const SharedNoiseModel& model) { NonlinearFactorGraph g; From d2063d928e855a326af6e806398b8b2afd97fd6e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 24 Jul 2020 11:37:58 -0500 Subject: [PATCH 4/4] added backwards compatibility for parseVertex --- gtsam/slam/dataset.cpp | 7 +++++++ gtsam/slam/dataset.h | 10 ++++++++++ 2 files changed, 17 insertions(+) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index cb15f84a8..4a95b4297 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -195,6 +195,13 @@ 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); +} +#endif + /* ************************************************************************* */ boost::optional parseVertexPose(istream& is, const string& tag) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 439a69fdc..7029a7a9c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -79,6 +79,16 @@ 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