DSF with Map, allows more general types

.cproject

@@ -487,6 +487,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testDSFMap.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam/base/tests/testDSFVector.cpp

@@ -33,13 +33,13 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-TEST(DSFVectorVector, find) {
+TEST(DSFBase, find) {
   DSFBase dsf(3);
   EXPECT(dsf.find(0) != dsf.find(2));
 }
 
 /* ************************************************************************* */
-TEST(DSFVectorVector, merge) {
+TEST(DSFBase, merge) {
   DSFBase dsf(3);
   dsf.merge(0,2);
   EXPECT(dsf.find(0) == dsf.find(2));

gtsam_unstable/base/DSF.h

@@ -18,12 +18,12 @@
 
 #pragma once
 
+#include <gtsam_unstable/base/BTree.h>
+#include <boost/foreach.hpp>
 #include <iostream>
 #include <list>
 #include <set>
 #include <map>
-#include <boost/foreach.hpp>
-#include <gtsam_unstable/base/BTree.h>
 
 namespace gtsam {
 
@@ -40,11 +40,10 @@ namespace gtsam {
   class DSF : protected BTree<KEY, KEY> {
 
   public:
-    typedef KEY Label; // label can be different from key, but for now they are same
     typedef DSF<KEY> Self;
     typedef std::set<KEY> Set;
-    typedef BTree<KEY, Label> Tree;
+    typedef BTree<KEY, KEY> Tree;
-    typedef std::pair<KEY, Label> KeyLabel;
+    typedef std::pair<KEY, KEY> KeyLabel;
 
     // constructor
     DSF() : Tree() { }
@@ -62,7 +61,7 @@ namespace gtsam {
     Self makeSet(const KEY& key) const { if (this->mem(key)) return *this; else return this->add(key, key); }
 
     // find the label of the set in which {key} lives
-    Label findSet(const KEY& key) const {
+    KEY findSet(const KEY& key) const {
       KEY parent = this->find(key);
       return parent == key ? key : findSet(parent); }
 
@@ -111,23 +110,23 @@ namespace gtsam {
     size_t size() const { return Tree::size(); }
 
     // return all sets, i.e. a partition of all elements
-    std::map<Label, Set> sets() const {
+    std::map<KEY, Set> sets() const {
-      std::map<Label, Set> sets;
+      std::map<KEY, Set> sets;
       BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
         sets[findSet(pair.second)].insert(pair.first);
       return sets;
     }
 
     // return a partition of the given elements {keys}
-    std::map<Label, Set> partition(const std::list<KEY>& keys) const {
+    std::map<KEY, Set> partition(const std::list<KEY>& keys) const {
-      std::map<Label, Set> partitions;
+      std::map<KEY, Set> partitions;
       BOOST_FOREACH(const KEY& key, keys)
         partitions[findSet(key)].insert(key);
       return partitions;
     }
 
     // get the nodes in the tree with the given label
-    Set set(const Label& label) const {
+    Set set(const KEY& label) const {
       Set set;
       BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) {
         if (pair.second == label || findSet(pair.second) == label)

gtsam_unstable/base/DSFMap.h

@@ -0,0 +1,65 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file DSFMap.h
+ * @date Oct 26, 2013
+ * @author Frank Dellaert
+ * @brief Allow for arbitrary type in DSF
+ */
+
+#pragma once
+
+#include <map>
+
+namespace gtsam {
+
+/**
+ * Disjoint set forest using an STL map data structure underneath
+ * Uses rank compression but not union by rank :-(
+ * @addtogroup base
+ */
+template<class KEY>
+class DSFMap {
+
+  /// We store the forest in an STL map
+  typedef std::map<KEY, KEY> Map;
+  mutable Map parent_;
+
+public:
+  /// constructor
+  DSFMap() {}
+
+  /// find the label of the set in which {key} lives
+  KEY find(const KEY& key) const {
+    typename Map::const_iterator it = parent_.find(key);
+    // if key does not exist, create and return itself
+    if (it==parent_.end()) {
+      parent_[key] = key;
+      return key;
+    } else {
+      // follow parent pointers until we reach set representative
+      KEY parent = it->second;
+      if (parent != key)
+        parent = find(parent); // not yet, recurse!
+      parent_[key] = parent; // path compression
+      return parent;
+    }
+  }
+
+  /// Merge two sets
+  void merge(const KEY& i1, const KEY& i2) {
+    parent_[find(i2)] = find(i1);
+  }
+
+};
+
+}

gtsam_unstable/base/tests/testDSFMap.cpp

@@ -0,0 +1,122 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testDSFMap.cpp
+ * @date Oct 26, 2013
+ * @author Frank Dellaert
+ * @brief unit tests for DSFMap
+ */
+
+#include <gtsam_unstable/base/DSFMap.h>
+
+#include <boost/foreach.hpp>
+#include <boost/assign/std/list.hpp>
+//#include <boost/assign/std/set.hpp>
+using namespace boost::assign;
+//
+#include <CppUnitLite/TestHarness.h>
+//
+//#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(DSFMap, find) {
+  DSFMap<size_t> dsf;
+  EXPECT(dsf.find(0)==0);
+  EXPECT(dsf.find(2)==2);
+  EXPECT(dsf.find(0)==0);
+  EXPECT(dsf.find(2)==2);
+  EXPECT(dsf.find(0) != dsf.find(2));
+}
+
+/* ************************************************************************* */
+TEST(DSFMap, merge) {
+  DSFMap<size_t> dsf;
+  dsf.merge(0,2);
+  EXPECT(dsf.find(0) == dsf.find(2));
+}
+/* ************************************************************************* */
+TEST(DSFMap, merge2) {
+  DSFMap<size_t> dsf;
+  dsf.merge(2,0);
+  EXPECT(dsf.find(0) == dsf.find(2));
+}
+
+/* ************************************************************************* */
+TEST(DSFMap, merge3) {
+  DSFMap<size_t> dsf;
+  dsf.merge(0,1);
+  dsf.merge(1,2);
+  EXPECT(dsf.find(0) == dsf.find(2));
+}
+
+/* ************************************************************************* */
+TEST(DSFMap, mergePairwiseMatches) {
+
+  // Create some "matches"
+  typedef pair<size_t,size_t> Match;
+  list<Match> matches;
+  matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6);
+
+  // Merge matches
+  DSFMap<size_t> dsf;
+  BOOST_FOREACH(const Match& m, matches)
+    dsf.merge(m.first,m.second);
+
+  // Each point is now associated with a set, represented by one of its members
+  size_t rep1 = 1, rep2 = 4;
+  EXPECT_LONGS_EQUAL(rep1,dsf.find(1));
+  EXPECT_LONGS_EQUAL(rep1,dsf.find(2));
+  EXPECT_LONGS_EQUAL(rep1,dsf.find(3));
+  EXPECT_LONGS_EQUAL(rep2,dsf.find(4));
+  EXPECT_LONGS_EQUAL(rep2,dsf.find(5));
+  EXPECT_LONGS_EQUAL(rep2,dsf.find(6));
+}
+
+/* ************************************************************************* */
+TEST(DSFMap, mergePairwiseMatches2) {
+
+  // Create some measurements with image index and feature index
+  typedef pair<size_t,size_t> Measurement;
+  Measurement m11(1,1),m12(1,2),m14(1,4); // in image 1
+  Measurement m22(2,2),m23(2,3),m25(2,5),m26(2,6); // in image 2
+
+  // Add them all
+  list<Measurement> measurements;
+  measurements += m11,m12,m14, m22,m23,m25,m26;
+
+  // Create some "matches"
+  typedef pair<Measurement,Measurement> Match;
+  list<Match> matches;
+  matches += Match(m11,m22), Match(m12,m23), Match(m14,m25), Match(m14,m26);
+
+  // Merge matches
+  DSFMap<Measurement> dsf;
+  BOOST_FOREACH(const Match& m, matches)
+    dsf.merge(m.first,m.second);
+
+  // Check that sets are merged correctly
+  EXPECT(dsf.find(m11)==m11);
+  EXPECT(dsf.find(m12)==m12);
+  EXPECT(dsf.find(m14)==m14);
+  EXPECT(dsf.find(m22)==m11);
+  EXPECT(dsf.find(m23)==m12);
+  EXPECT(dsf.find(m25)==m14);
+  EXPECT(dsf.find(m26)==m14);
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+/* ************************************************************************* */
+