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