Cleaned up DSFVector and added pairwise matches example

.cproject

@@ -1787,6 +1787,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testDSFVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testVectorValuesUnordered.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>

gtsam/base/DSFVector.cpp

@@ -16,38 +16,50 @@
  * @brief a faster implementation for DSF, which uses vector rather than btree.
  */
 
+#include <gtsam/base/DSFVector.h>
 #include <boost/make_shared.hpp>
 #include <boost/foreach.hpp>
-#include <gtsam/base/DSFVector.h>
+#include <algorithm>
 
 using namespace std;
 
 namespace gtsam {
 
 /* ************************************************************************* */
-DSFVector::DSFVector (const size_t numNodes) {
+DSFVector::DSFVector(const size_t numNodes) {
-  v_ = boost::make_shared<V>(numNodes);
+  v_ = boost::make_shared < V > (numNodes);
   int index = 0;
   keys_.reserve(numNodes);
-  for(V::iterator it = v_->begin(); it!=v_->end(); it++, index++) {
+  for (V::iterator it = v_->begin(); it != v_->end(); it++, index++) {
     *it = index;
     keys_.push_back(index);
   }
 }
 
 /* ************************************************************************* */
-DSFVector::DSFVector(const boost::shared_ptr<V>& v_in, const std::vector<size_t>& keys) : keys_(keys) {
+DSFVector::DSFVector(const std::vector<size_t>& keys) :
+    keys_(keys) {
+  size_t maxKey = *(std::max_element(keys.begin(), keys.end()));
+  v_ = boost::make_shared < V > (maxKey + 1);
+  BOOST_FOREACH(const size_t key, keys)
+    (*v_)[key] = key;
+}
+
+/* ************************************************************************* */
+DSFVector::DSFVector(const boost::shared_ptr<V>& v_in,
+    const std::vector<size_t>& keys) :
+    keys_(keys) {
+  assert(*(std::max_element(keys.begin(), keys.end()))<v_in->size());
   v_ = v_in;
   BOOST_FOREACH(const size_t key, keys)
     (*v_)[key] = key;
 }
 
 /* ************************************************************************* */
-bool DSFVector::isSingleton(const Label& label) const {
+bool DSFVector::isSingleton(const size_t& label) const {
   bool result = false;
-  V::const_iterator it = keys_.begin();
+  BOOST_FOREACH(size_t key,keys_) {
-  for (; it != keys_.end(); ++it) {
+    if (findSet(key) == label) {
-    if(findSet(*it) == label) {
       if (!result) // find the first occurrence
         result = true;
       else
@@ -58,33 +70,27 @@ bool DSFVector::isSingleton(const Label& label) const {
 }
 
 /* ************************************************************************* */
-std::set<size_t> DSFVector::set(const Label& label) const {
+std::set<size_t> DSFVector::set(const size_t& label) const {
-  std::set<size_t> set;
+  std::set < size_t > set;
-  V::const_iterator it = keys_.begin();
+  BOOST_FOREACH(size_t key,keys_)
-  for (; it != keys_.end(); it++) {
+    if (findSet(key) == label)
-    if (findSet(*it) == label)
+      set.insert(key);
-      set.insert(*it);
-  }
   return set;
 }
 
 /* ************************************************************************* */
-std::map<DSFVector::Label, std::set<size_t> > DSFVector::sets() const {
+std::map<size_t, std::set<size_t> > DSFVector::sets() const {
-  std::map<Label, std::set<size_t> > sets;
+  std::map<size_t, std::set<size_t> > sets;
-  V::const_iterator it = keys_.begin();
+  BOOST_FOREACH(size_t key,keys_)
-  for (; it != keys_.end(); it++) {
+    sets[findSet(key)].insert(key);
-    sets[findSet(*it)].insert(*it);
-  }
   return sets;
 }
 
 /* ************************************************************************* */
-std::map<DSFVector::Label, std::vector<size_t> > DSFVector::arrays() const {
+std::map<size_t, std::vector<size_t> > DSFVector::arrays() const {
-  std::map<Label, std::vector<size_t> > arrays;
+  std::map<size_t, std::vector<size_t> > arrays;
-  V::const_iterator it = keys_.begin();
+  BOOST_FOREACH(size_t key,keys_)
-  for (; it != keys_.end(); it++) {
+    arrays[findSet(key)].push_back(key);
-    arrays[findSet(*it)].push_back(*it);
-  }
   return arrays;
 }
 

gtsam/base/DSFVector.h

@@ -18,42 +18,41 @@
 
 #pragma once
 
-#include <vector>
-#include <map>
-#include <set>
-#include <boost/shared_ptr.hpp>
-
 #include <gtsam/global_includes.h>
+#include <boost/shared_ptr.hpp>
+#include <vector>
+#include <set>
+#include <map>
 
 namespace gtsam {
 
-  /**
+/**
  * A fast implementation of disjoint set forests that uses vector as underly data structure.
  * @addtogroup base
  */
-  class GTSAM_EXPORT DSFVector {
+class GTSAM_EXPORT DSFVector {
 
-  public:
+public:
   typedef std::vector<size_t> V; ///< Vector of ints
-    typedef size_t Label;          ///< Label type
-    typedef V::const_iterator const_iterator; ///< const iterator over V
-    typedef V::iterator iterator;///< iterator over V
 
-  private:
+private:
-     // TODO could use existing memory to improve the efficiency
+  boost::shared_ptr<V> v_;///< Stores parent pointers, representative iff v[i]==i
-    boost::shared_ptr<V> v_;
+  std::vector<size_t> keys_;///< stores keys
-    std::vector<size_t> keys_;
 
-  public:
+public:
-    /// constructor that allocate a new memory
+  /// constructor that allocate new memory, uses sequential keys 0...numNodes-1
   DSFVector(const size_t numNodes);
 
+  /// constructor that allocates memory, uses given keys
+  DSFVector(const std::vector<size_t>& keys);
+
   /// constructor that uses the existing memory
   DSFVector(const boost::shared_ptr<V>& v_in, const std::vector<size_t>& keys);
 
   /// find the label of the set in which {key} lives
-    inline Label findSet(size_t key) const {
+  inline size_t findSet(size_t key) const {
     size_t parent = (*v_)[key];
+    // follow parent pointers until we reach set representative
     while (parent != key) {
       key = parent;
       parent = (*v_)[key];
@@ -62,20 +61,19 @@ namespace gtsam {
   }
 
   /// find whether there is one and only one occurrence for the given {label}
-    bool isSingleton(const Label& label) const;
+  bool isSingleton(const size_t& label) const;
 
   /// get the nodes in the tree with the given label
-    std::set<size_t> set(const Label& label) const;
+  std::set<size_t> set(const size_t& label) const;
 
   /// return all sets, i.e. a partition of all elements
-    std::map<Label, std::set<size_t> > sets() const;
+  std::map<size_t, std::set<size_t> > sets() const;
 
   /// return all sets, i.e. a partition of all elements
-    std::map<Label, std::vector<size_t> > arrays() const;
+  std::map<size_t, std::vector<size_t> > arrays() const;
 
   /// the in-place version of makeUnion
   void makeUnionInPlace(const size_t& i1, const size_t& i2);
-
+};
-  };
 
 }

gtsam/base/tests/testDSFVector.cpp

@@ -16,15 +16,18 @@
  * @brief unit tests for DSF
  */
 
-#include <iostream>
+#include <gtsam/base/DSFVector.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/foreach.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/assign/std/list.hpp>
 #include <boost/assign/std/set.hpp>
 #include <boost/assign/std/vector.hpp>
 using namespace boost::assign;
-#include <CppUnitLite/TestHarness.h>
 
-#include <gtsam/base/DSFVector.h>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;
@@ -32,14 +35,14 @@ using namespace gtsam;
 /* ************************************************************************* */
 TEST(DSFVectorVector, findSet) {
   DSFVector dsf(3);
-  CHECK(dsf.findSet(0) != dsf.findSet(2));
+  EXPECT(dsf.findSet(0) != dsf.findSet(2));
 }
 
 /* ************************************************************************* */
 TEST(DSFVectorVector, makeUnionInPlace) {
   DSFVector dsf(3);
   dsf.makeUnionInPlace(0,2);
-  CHECK(dsf.findSet(0) == dsf.findSet(2));
+  EXPECT(dsf.findSet(0) == dsf.findSet(2));
 }
 
 /* ************************************************************************* */
@@ -48,14 +51,14 @@ TEST(DSFVectorVector, makeUnionInPlace2) {
   std::vector<size_t> keys; keys += 1, 3;
   DSFVector dsf(v, keys);
   dsf.makeUnionInPlace(1,3);
-  CHECK(dsf.findSet(1) == dsf.findSet(3));
+  EXPECT(dsf.findSet(1) == dsf.findSet(3));
 }
 
 /* ************************************************************************* */
 TEST(DSFVector, makeUnion2) {
   DSFVector dsf(3);
   dsf.makeUnionInPlace(2,0);
-  CHECK(dsf.findSet(0) == dsf.findSet(2));
+  EXPECT(dsf.findSet(0) == dsf.findSet(2));
 }
 
 /* ************************************************************************* */
@@ -63,7 +66,7 @@ TEST(DSFVector, makeUnion3) {
   DSFVector dsf(3);
   dsf.makeUnionInPlace(0,1);
   dsf.makeUnionInPlace(1,2);
-  CHECK(dsf.findSet(0) == dsf.findSet(2));
+  EXPECT(dsf.findSet(0) == dsf.findSet(2));
 }
 
 /* ************************************************************************* */
@@ -74,7 +77,7 @@ TEST(DSFVector, sets) {
   LONGS_EQUAL(1, sets.size());
 
   set<size_t> expected; expected += 0, 1;
-  CHECK(expected == sets[dsf.findSet(0)]);
+  EXPECT(expected == sets[dsf.findSet(0)]);
 }
 
 /* ************************************************************************* */
@@ -85,7 +88,7 @@ TEST(DSFVector, arrays) {
   LONGS_EQUAL(1, arrays.size());
 
   vector<size_t> expected; expected += 0, 1;
-  CHECK(expected == arrays[dsf.findSet(0)]);
+  EXPECT(expected == arrays[dsf.findSet(0)]);
 }
 
 /* ************************************************************************* */
@@ -97,7 +100,7 @@ TEST(DSFVector, sets2) {
   LONGS_EQUAL(1, sets.size());
 
   set<size_t> expected; expected += 0, 1, 2;
-  CHECK(expected == sets[dsf.findSet(0)]);
+  EXPECT(expected == sets[dsf.findSet(0)]);
 }
 
 /* ************************************************************************* */
@@ -109,7 +112,7 @@ TEST(DSFVector, arrays2) {
   LONGS_EQUAL(1, arrays.size());
 
   vector<size_t> expected; expected += 0, 1, 2;
-  CHECK(expected == arrays[dsf.findSet(0)]);
+  EXPECT(expected == arrays[dsf.findSet(0)]);
 }
 
 /* ************************************************************************* */
@@ -120,7 +123,7 @@ TEST(DSFVector, sets3) {
   LONGS_EQUAL(2, sets.size());
 
   set<size_t> expected; expected += 0, 1;
-  CHECK(expected == sets[dsf.findSet(0)]);
+  EXPECT(expected == sets[dsf.findSet(0)]);
 }
 
 /* ************************************************************************* */
@@ -131,7 +134,7 @@ TEST(DSFVector, arrays3) {
   LONGS_EQUAL(2, arrays.size());
 
   vector<size_t> expected; expected += 0, 1;
-  CHECK(expected == arrays[dsf.findSet(0)]);
+  EXPECT(expected == arrays[dsf.findSet(0)]);
 }
 
 /* ************************************************************************* */
@@ -142,7 +145,7 @@ TEST(DSFVector, set) {
   LONGS_EQUAL(2, set.size());
 
   std::set<size_t> expected; expected += 0, 1;
-  CHECK(expected == set);
+  EXPECT(expected == set);
 }
 
 /* ************************************************************************* */
@@ -154,17 +157,61 @@ TEST(DSFVector, set2) {
   LONGS_EQUAL(3, set.size());
 
   std::set<size_t> expected; expected += 0, 1, 2;
-  CHECK(expected == set);
+  EXPECT(expected == set);
 }
 
 /* ************************************************************************* */
 TEST(DSFVector, isSingleton) {
   DSFVector dsf(3);
   dsf.makeUnionInPlace(0,1);
-  CHECK(!dsf.isSingleton(0));
+  EXPECT(!dsf.isSingleton(0));
-  CHECK(!dsf.isSingleton(1));
+  EXPECT(!dsf.isSingleton(1));
-  CHECK( dsf.isSingleton(2));
+  EXPECT( dsf.isSingleton(2));
 }
+
+/* ************************************************************************* */
+TEST(DSFVector, mergePairwiseMatches) {
+
+  // Create some measurements
+  vector<size_t> keys;
+  keys += 1,2,3,4,5,6;
+
+  // Create some "matches"
+  typedef pair<size_t,size_t> Match;
+  vector<Match> matches;
+  matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6);
+
+  // Merge matches
+  DSFVector dsf(keys);
+  BOOST_FOREACH(const Match& m, matches)
+    dsf.makeUnionInPlace(m.first,m.second);
+
+  cout << endl;
+  map<size_t, set<size_t> > sets1 = dsf.sets();
+
+  // 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.findSet(1));
+  EXPECT_LONGS_EQUAL(rep1,dsf.findSet(2));
+  EXPECT_LONGS_EQUAL(rep1,dsf.findSet(3));
+  EXPECT_LONGS_EQUAL(rep2,dsf.findSet(4));
+  EXPECT_LONGS_EQUAL(rep2,dsf.findSet(5));
+  EXPECT_LONGS_EQUAL(rep2,dsf.findSet(6));
+
+  // Check that we have two connected components, 1,2,3 and 4,5,6
+  cout << endl;
+  map<size_t, set<size_t> > sets = dsf.sets();
+  LONGS_EQUAL(2, sets.size());
+  set<size_t> expected1; expected1 += 1,2,3;
+  set<size_t> actual1 = sets[rep1];
+  BOOST_FOREACH(size_t i, actual1) cout << i << " ";
+  EXPECT(expected1 == actual1);
+  set<size_t> expected2; expected2 += 4,5,6;
+  set<size_t> actual2 = sets[rep2];
+  BOOST_FOREACH(size_t i, actual2) cout << i << " ";
+  EXPECT(expected2 == actual2);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */