From 724a906bee985b3c7b3141fce81dd72141163094 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Mar 2019 22:47:23 -0400 Subject: [PATCH 1/5] Test existing readG2o --- cython/gtsam/tests/test_dataset.py | 33 ++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 cython/gtsam/tests/test_dataset.py diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py new file mode 100644 index 000000000..c634fb3f7 --- /dev/null +++ b/cython/gtsam/tests/test_dataset.py @@ -0,0 +1,33 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for testing dataset access. +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import unittest + +import gtsam + + +class TestDataset(unittest.TestCase): + def setUp(self): + pass + + def test_3d_graph(self): + is3D = True + g2o_file = gtsam.findExampleDataFile("pose3example.txt") + graph, initial = gtsam.readG2o(g2o_file, is3D) + self.assertEqual(graph.size(), 6) + self.assertEqual(initial.size(), 5) + + +if __name__ == '__main__': + unittest.main() From 0ca3f9d199e8900f119248a64a71694af27bcf17 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Mar 2019 23:22:37 -0400 Subject: [PATCH 2/5] Use c+11 initializer lists --- gtsam/slam/tests/testDataset.cpp | 91 +++++++++++++++----------------- 1 file changed, 44 insertions(+), 47 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 39c2d3722..4a327b8e1 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,65 +162,62 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ -TEST( dataSet, readG2o3D) -{ +TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = true; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - Values expectedValues; - Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); - Point3 p0 = Point3(0.000000, 0.000000, 0.000000); - expectedValues.insert(0, Pose3(R0, p0)); + // Initialize 5 poses with quaternion/point3 values: + const std::vector poses = { + {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.421446, -0.351729, -0.597838, 0.584174}, + {1.993500, 0.023275, 0.003793}}, + {{0.067024, 0.331798, -0.200659, 0.919323}, + {2.004291, 1.024305, 0.018047}}, + {{0.765488, -0.035697, -0.462490, 0.445933}, + {0.999908, 1.055073, 0.020212}}, + }; - Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - Point3 p1 = Point3(1.001367, 0.015390, 0.004948); - expectedValues.insert(1, Pose3(R1, p1)); + // Check values + for (size_t i : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[i], actualValues->at(i), 1e-5)); + } - Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); - Point3 p2 = Point3(1.993500, 0.023275, 0.003793); - expectedValues.insert(2, Pose3(R2, p2)); + // Initialize 6 relative measurements with quaternion/point3 values: + const std::vector measurements = { + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.105373, 0.311512, 0.656877, -0.678505}, + {0.523923, 0.776654, 0.326659}}, + {{0.568551, 0.595795, -0.561677, 0.079353}, + {0.910927, 0.055169, -0.411761}}, + {{0.542221, -0.592077, 0.303380, -0.513226}, + {0.775288, 0.228798, -0.596923}}, + {{0.327419, -0.125250, -0.534379, 0.769122}, + {-0.577841, 0.628016, -0.543592}}, + {{0.083672, 0.104639, 0.627755, 0.766795}, + {-0.623267, 0.086928, 0.773222}}, + }; - Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323); - Point3 p3 = Point3(2.004291, 1.024305, 0.018047); - expectedValues.insert(3, Pose3(R3, p3)); + // Specify connectivity: + std::vector> pairs = {{0, 1}, {1, 2}, {2, 3}, + {3, 4}, {1, 4}, {3, 0}}; - Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933); - Point3 p4 = Point3(0.999908, 1.055073, 0.020212); - expectedValues.insert(4, Pose3(R4, p4)); - - EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); - - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished()); + // Create expected factor graph + auto model = noiseModel::Isotropic::Precision(6, 10000); NonlinearFactorGraph expectedGraph; + size_t i = 0; + for (const auto& keys : pairs) { + expectedGraph.emplace_shared>( + keys.first, keys.second, measurements[i++], model); + } - Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); - - Point3 p12 = Point3(0.523923, 0.776654, 0.326659); - Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); - expectedGraph.emplace_shared >(1, 2, Pose3(R12,p12), model); - - Point3 p23 = Point3(0.910927, 0.055169, -0.411761); - Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); - expectedGraph.emplace_shared >(2, 3, Pose3(R23,p23), model); - - Point3 p34 = Point3(0.775288, 0.228798, -0.596923); - Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); - expectedGraph.emplace_shared >(3, 4, Pose3(R34,p34), model); - - Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); - Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); - expectedGraph.emplace_shared >(1, 4, Pose3(R14,p14), model); - - Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); - Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); - expectedGraph.emplace_shared >(3, 0, Pose3(R30,p30), model); - - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); + // Check if actual graph is correct + EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); } /* ************************************************************************* */ From d8ee79fb8fd747bf2d497bacb9c808cb8d1b5fb6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 00:27:02 -0400 Subject: [PATCH 3/5] Working parseG2o3D --- gtsam/slam/tests/testDataset.cpp | 167 +++++++++++++++++++++++++------ 1 file changed, 135 insertions(+), 32 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 4a327b8e1..3e2ed51c7 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,33 +162,96 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ +using BetweenFactors = std::vector::shared_ptr>; +using Poses = std::map; +std::pair parseG2o3D(const std::string&); + +#include +#define LINESIZE 81920 + +/* ************************************************************************* */ +std::pair parseG2o3D(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("load3D: can not find file " + filename); + + BetweenFactors factors; + Poses poses; + + 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(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); + } + } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + + 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); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); + } + } + return make_pair(factors, poses); +} + TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); - NonlinearFactorGraph::shared_ptr actualGraph; - Values::shared_ptr actualValues; - bool is3D = true; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - - // Initialize 5 poses with quaternion/point3 values: - const std::vector poses = { - {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, - {{0.854230, 0.190253, 0.283162, -0.392318}, - {1.001367, 0.015390, 0.004948}}, - {{0.421446, -0.351729, -0.597838, 0.584174}, - {1.993500, 0.023275, 0.003793}}, - {{0.067024, 0.331798, -0.200659, 0.919323}, - {2.004291, 1.024305, 0.018047}}, - {{0.765488, -0.035697, -0.462490, 0.445933}, - {0.999908, 1.055073, 0.020212}}, - }; - - // Check values - for (size_t i : {0, 1, 2, 3, 4}) { - EXPECT(assert_equal(poses[i], actualValues->at(i), 1e-5)); - } + auto model = noiseModel::Isotropic::Precision(6, 10000); // Initialize 6 relative measurements with quaternion/point3 values: - const std::vector measurements = { + const std::vector relative_poses = { {{0.854230, 0.190253, 0.283162, -0.392318}, {1.001367, 0.015390, 0.004948}}, {{0.105373, 0.311512, 0.656877, -0.678505}, @@ -203,21 +266,61 @@ TEST(dataSet, readG2o3D) { {-0.623267, 0.086928, 0.773222}}, }; - // Specify connectivity: - std::vector> pairs = {{0, 1}, {1, 2}, {2, 3}, - {3, 4}, {1, 4}, {3, 0}}; + // Initialize 5 poses with quaternion/point3 values: + const std::vector poses = { + {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.421446, -0.351729, -0.597838, 0.584174}, + {1.993500, 0.023275, 0.003793}}, + {{0.067024, 0.331798, -0.200659, 0.919323}, + {2.004291, 1.024305, 0.018047}}, + {{0.765488, -0.035697, -0.462490, 0.445933}, + {0.999908, 1.055073, 0.020212}}, + }; - // Create expected factor graph - auto model = noiseModel::Isotropic::Precision(6, 10000); - NonlinearFactorGraph expectedGraph; + // Specify connectivity: + using KeyPair = pair; + std::vector edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}}; + + // Created expected factor graph size_t i = 0; - for (const auto& keys : pairs) { + NonlinearFactorGraph expectedGraph; + for (const auto& keys : edges) { expectedGraph.emplace_shared>( - keys.first, keys.second, measurements[i++], model); + keys.first, keys.second, relative_poses[i++], model); } - // Check if actual graph is correct + // Call parse version + BetweenFactors actualFactors; + Poses actualPoses; + std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile); + + // Check factors + for (size_t i : {0, 1, 2, 3, 4, 5}) { + EXPECT(assert_equal( + *boost::dynamic_pointer_cast>(expectedGraph[i]), + *actualFactors[i], 1e-5)); + } + + // Check poses + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); + } + + // Call graph version + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + bool is3D = true; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); + + // Check factor graph EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); + + // Check values + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); + } } /* ************************************************************************* */ From a47c52cb5ee858fec44b4fde82734aff95e47f64 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 00:58:22 -0400 Subject: [PATCH 4/5] Split parsing and moved to dataset.* --- gtsam/slam/dataset.cpp | 100 ++++++++++++++++-------------- gtsam/slam/dataset.h | 10 ++- gtsam/slam/tests/testDataset.cpp | 101 ++----------------------------- 3 files changed, 67 insertions(+), 144 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 34370d4e2..b7d1b04d6 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, /* ************************************************************************* */ GraphAndValues readG2o(const string& g2oFile, const bool is3D, - KernelFunctionType kernelFunctionType) { - // just call load2D - int maxID = 0; - bool addNoise = false; - bool smart = true; - - if(is3D) + KernelFunctionType kernelFunctionType) { + if (is3D) { return load3D(g2oFile); - - return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, - NoiseFormatG2O, kernelFunctionType); + } else { + // just call load2D + int maxID = 0; + bool addNoise = false; + bool smart = true; + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + NoiseFormatG2O, kernelFunctionType); + } } /* ************************************************************************* */ @@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } /* ************************************************************************* */ -GraphAndValues load3D(const string& filename) { - +std::map parse3DPoses(const string& filename) { ifstream is(filename.c_str()); if (!is) - throw invalid_argument("load3D: can not find file " + filename); - - Values::shared_ptr initial(new Values); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + throw invalid_argument("parse3DPoses: can not find file " + filename); + std::map poses; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + 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; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); } } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); + return poses; +} +/* ************************************************************************* */ +std::vector::shared_ptr> parse3DFactors(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); + + std::vector::shared_ptr> factors; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - Matrix m = I_6x6; - for (int i = 0; i < 6; i++) - for (int j = i; j < 6; j++) - ls >> m(i, j); + 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); - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = I_6x6; Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - for (int i = 0; i < 6; i++){ - for (int j = i; j < 6; j++){ + 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 = I_6x6; + 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 + 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); - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); } } + return factors; +} + +/* ************************************************************************* */ +GraphAndValues load3D(const string& filename) { + const auto factors = parse3DFactors(filename); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + for (const auto& factor : factors) { + graph->push_back(factor); + } + + const auto poses = parse3DPoses(filename); + Values::shared_ptr initial(new Values); + for (const auto& key_pose : poses) { + initial->insert(key_pose.first, key_pose.second); + } + return make_pair(graph, initial); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7c9b54a6f..9706bace0 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -153,9 +153,13 @@ 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); -/** - * Load TORO 3D Graph - */ +/// Parse edges in 3D TORO graph file into a set of BetweenFactors. +GTSAM_EXPORT std::vector::shared_ptr> parse3DFactors(const std::string& filename); + +/// Parse vertices in 3D TORO graph file into a map of Pose3s. +GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); + +/// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 3e2ed51c7..0e131af32 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,90 +162,6 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ -using BetweenFactors = std::vector::shared_ptr>; -using Poses = std::map; -std::pair parseG2o3D(const std::string&); - -#include -#define LINESIZE 81920 - -/* ************************************************************************* */ -std::pair parseG2o3D(const string& filename) { - ifstream is(filename.c_str()); - if (!is) throw invalid_argument("load3D: can not find file " + filename); - - BetweenFactors factors; - Poses poses; - - 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(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); - } - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - - 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); - factors.emplace_back(new BetweenFactor( - id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); - } - } - return make_pair(factors, poses); -} - TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); auto model = noiseModel::Isotropic::Precision(6, 10000); @@ -291,33 +207,26 @@ TEST(dataSet, readG2o3D) { keys.first, keys.second, relative_poses[i++], model); } - // Call parse version - BetweenFactors actualFactors; - Poses actualPoses; - std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile); - - // Check factors + // Check factor parsing + const auto actualFactors = parse3DFactors(g2oFile); for (size_t i : {0, 1, 2, 3, 4, 5}) { EXPECT(assert_equal( *boost::dynamic_pointer_cast>(expectedGraph[i]), *actualFactors[i], 1e-5)); } - // Check poses + // Check pose parsing + const auto actualPoses = parse3DPoses(g2oFile); for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); } - // Call graph version + // Check graph version NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = true; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - - // Check factor graph EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); - - // Check values for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); } From 88ac6de4afb1cef6ac6837f1bd33280c9f0cfb5d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 01:25:06 -0400 Subject: [PATCH 5/5] Wrapped parse3DFactors --- cython/gtsam/tests/test_dataset.py | 23 +++++++++++++++++------ gtsam.h | 10 ++++++++++ gtsam/slam/dataset.cpp | 2 +- gtsam/slam/dataset.h | 5 ++++- gtsam/slam/tests/testDataset.cpp | 2 +- 5 files changed, 33 insertions(+), 9 deletions(-) diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py index c634fb3f7..e561be1a8 100644 --- a/cython/gtsam/tests/test_dataset.py +++ b/cython/gtsam/tests/test_dataset.py @@ -8,26 +8,37 @@ See LICENSE for the license information Unit tests for testing dataset access. Author: Frank Dellaert """ -# pylint: disable=invalid-name, E1101 +# pylint: disable=invalid-name, no-name-in-module, no-member from __future__ import print_function import unittest import gtsam +from gtsam import BetweenFactorPose3, BetweenFactorPose3s class TestDataset(unittest.TestCase): - def setUp(self): - pass + """Tests for datasets.h wrapper.""" - def test_3d_graph(self): + def setUp(self): + """Get some common paths.""" + self.pose3_example_g2o_file = gtsam.findExampleDataFile( + "pose3example.txt") + + def test_readG2o3D(self): + """Test reading directly into factor graph.""" is3D = True - g2o_file = gtsam.findExampleDataFile("pose3example.txt") - graph, initial = gtsam.readG2o(g2o_file, is3D) + graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D) self.assertEqual(graph.size(), 6) self.assertEqual(initial.size(), 5) + def test_parse3Dfactors(self): + """Test parsing into data structure.""" + factors = gtsam.parse3DFactors(self.pose3_example_g2o_file) + self.assertEqual(factors.size(), 6) + self.assertIsInstance(factors.at(0), BetweenFactorPose3) + if __name__ == '__main__': unittest.main() diff --git a/gtsam.h b/gtsam.h index 48768db40..255a7a449 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2480,6 +2480,16 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); +// std::vector::shared_ptr> +class BetweenFactorPose3s +{ + size_t size() const; + gtsam::BetweenFactorPose3* at(size_t i) const; +}; + +gtsam::BetweenFactorPose3s parse3DFactors(string filename); +pair load3D(string filename); + pair readG2o(string filename); pair readG2o(string filename, bool is3D); void writeG2o(const gtsam::NonlinearFactorGraph& graph, diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b7d1b04d6..c32a049e2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -540,7 +540,7 @@ std::map parse3DPoses(const string& filename) { } /* ************************************************************************* */ -std::vector::shared_ptr> parse3DFactors(const string& filename) { +BetweenFactorPose3s parse3DFactors(const string& filename) { ifstream is(filename.c_str()); if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 9706bace0..2106df48d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -34,6 +35,7 @@ #include // for pair #include #include +#include namespace gtsam { @@ -154,7 +156,8 @@ 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. -GTSAM_EXPORT std::vector::shared_ptr> parse3DFactors(const std::string& filename); +using BetweenFactorPose3s = std::vector::shared_ptr>; +GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename); /// Parse vertices in 3D TORO graph file into a map of Pose3s. GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 0e131af32..9434280d4 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -16,8 +16,8 @@ */ -#include #include +#include #include #include #include