diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py new file mode 100644 index 000000000..e561be1a8 --- /dev/null +++ b/cython/gtsam/tests/test_dataset.py @@ -0,0 +1,44 @@ +""" +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, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import BetweenFactorPose3, BetweenFactorPose3s + + +class TestDataset(unittest.TestCase): + """Tests for datasets.h wrapper.""" + + 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 + 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 50d59f973..13e0474d5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2498,6 +2498,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 34370d4e2..c32a049e2 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; +} +/* ************************************************************************* */ +BetweenFactorPose3s 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..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 { @@ -153,9 +155,14 @@ 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. +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); + +/// 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 39c2d3722..9434280d4 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -16,8 +16,8 @@ */ -#include #include +#include #include #include #include @@ -162,65 +162,74 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ -TEST( dataSet, readG2o3D) -{ +TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); + auto model = noiseModel::Isotropic::Precision(6, 10000); + + // Initialize 6 relative measurements with quaternion/point3 values: + 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}, + {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}}, + }; + + // 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}}, + }; + + // 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; + NonlinearFactorGraph expectedGraph; + for (const auto& keys : edges) { + expectedGraph.emplace_shared>( + keys.first, keys.second, relative_poses[i++], model); + } + + // 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 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)); + } + + // Check graph version 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)); - - 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)); - - 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)); - - 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)); - - 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()); - NonlinearFactorGraph expectedGraph; - - 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)); + EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); + } } /* ************************************************************************* */