Merge pull request #511 from borglab/feature/wrap_dsf

Wrap the DSFMap class so SFM can use them
borglab · Sep 7, 2020 · bf7213f · bf7213f
2 parents 8b1516f + 00fbea7
commit bf7213f
Show file tree

Hide file tree

Showing 6 changed files with 81 additions and 5 deletions.
diff --git a/gtsam/base/DSFMap.h b/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
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
@@ -259,11 +259,59 @@ class IndexPair {
   size_t j() const;
 };
 
-template<KEY = {gtsam::IndexPair}>
-class DSFMap {
-  DSFMap();
-  KEY find(const KEY& key) const;
-  void merge(const KEY& x, const KEY& y);
+// template<KEY = {gtsam::IndexPair}>
+// class DSFMap {
+//   DSFMap();
+//   KEY find(const KEY& key) const;
+//   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>

diff --git a/python/CMakeLists.txt b/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

diff --git a/python/gtsam/preamble.h b/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>);
diff --git a/python/gtsam/specializations.h b/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");
diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py
@@ -35,6 +35,20 @@ def key(index_pair):
         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()