Merge pull request #511 from borglab/feature/wrap_dsf

Wrap the DSFMap class so SFM can use them

gtsam/base/DSFMap.h

@@ -21,6 +21,7 @@
 #include <cstdlib>  // Provides size_t
 #include <map>
 #include <set>
+#include <vector>
 
 namespace gtsam {
 
@@ -120,4 +121,12 @@ class IndexPair : public std::pair<size_t,size_t> {
   inline size_t i() const { return first; };
   inline size_t j() const { return second; };
 };
+
+typedef std::vector<IndexPair> IndexPairVector;
+typedef std::set<IndexPair> IndexPairSet;
+
+inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); }
+
+typedef std::map<IndexPair, IndexPairSet> IndexPairSetMap;
+typedef DSFMap<IndexPair> DSFMapIndexPair;
 }  // namespace gtsam

gtsam/gtsam.i

@@ -259,11 +259,59 @@ class IndexPair {
   size_t j() const;
 };
 
-template<KEY = {gtsam::IndexPair}>
+// template<KEY = {gtsam::IndexPair}>
-class DSFMap {
+// class DSFMap {
-  DSFMap();
+//   DSFMap();
-  KEY find(const KEY& key) const;
+//   KEY find(const KEY& key) const;
-  void merge(const KEY& x, const KEY& y);
+//   void merge(const KEY& x, const KEY& y);
+//   std::map<KEY, Set> sets();
+// };
+
+class IndexPairSet {
+  IndexPairSet();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(gtsam::IndexPair key);
+  bool erase(gtsam::IndexPair key); // returns true if value was removed
+  bool count(gtsam::IndexPair key) const; // returns true if value exists
+};
+
+class IndexPairVector {
+  IndexPairVector();
+  IndexPairVector(const gtsam::IndexPairVector& other);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPair at(size_t i) const;
+  void push_back(gtsam::IndexPair key) const;
+};
+
+gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
+
+class IndexPairSetMap {
+  IndexPairSetMap();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPairSet at(gtsam::IndexPair& key);
+};
+
+class DSFMapIndexPair {
+  DSFMapIndexPair();
+  gtsam::IndexPair find(const gtsam::IndexPair& key) const;
+  void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y);
+  gtsam::IndexPairSetMap sets();
 };
 
 #include <gtsam/base/Matrix.h>

python/CMakeLists.txt

@@ -30,6 +30,8 @@ set(ignore
     gtsam::ISAM2ThresholdMapValue
     gtsam::FactorIndices
     gtsam::FactorIndexSet
+    gtsam::IndexPairSetMap
+    gtsam::IndexPairVector
     gtsam::BetweenFactorPose2s
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector

python/gtsam/preamble.h

@@ -9,3 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);

python/gtsam/specializations.h

@@ -9,3 +9,5 @@ py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point
 py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
 py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
 py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");
+py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
+py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");

python/gtsam/tests/test_dsf_map.py

@@ -35,6 +35,20 @@ class TestDSFMap(GtsamTestCase):
         dsf.merge(pair1, pair2)
         self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
 
+    def test_sets(self):
+        from gtsam import IndexPair
+        dsf = gtsam.DSFMapIndexPair()
+        dsf.merge(IndexPair(0, 1), IndexPair(1,2))
+        dsf.merge(IndexPair(0, 1), IndexPair(3,4))
+        dsf.merge(IndexPair(4,5), IndexPair(6,8))
+        sets = dsf.sets()
+
+        for i in sets:
+            s = sets[i]
+            for val in gtsam.IndexPairSetAsArray(s):
+                val.i()
+                val.j()
+
 
 if __name__ == '__main__':
     unittest.main()