Wrapped parse3DFactors

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()

gtsam.h

@@ -2480,6 +2480,16 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
     string filename);
 
+// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
+class BetweenFactorPose3s
+{
+  size_t size() const;
+  gtsam::BetweenFactorPose3* at(size_t i) const;
+};
+
+gtsam::BetweenFactorPose3s parse3DFactors(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
+
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,

gtsam/slam/dataset.cpp

@@ -540,7 +540,7 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
 }
 
 /* ************************************************************************* */
-std::vector<BetweenFactor<Pose3>::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);
 

gtsam/slam/dataset.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
@@ -34,6 +35,7 @@
 #include <utility> // for pair
 #include <vector>
 #include <iosfwd>
+#include <map>
 
 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<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const std::string& filename);
+using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::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<Key, Pose3> parse3DPoses(const std::string& filename);

gtsam/slam/tests/testDataset.cpp

@@ -16,8 +16,8 @@
  */
 
 
-#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/base/TestableAssertions.h>