diff --git a/.cproject b/.cproject
index fed8aa1f5..46d6f3844 100644
--- a/.cproject
+++ b/.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>
diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp
index 2bf18916d..929835cfa 100644
--- a/gtsam/base/DSFVector.cpp
+++ b/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) {
-  v_ = boost::make_shared<V>(numNodes);
+DSFVector::DSFVector(const size_t 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();
-  for (; it != keys_.end(); ++it) {
-    if(findSet(*it) == label) {
+  BOOST_FOREACH(size_t key,keys_) {
+    if (findSet(key) == label) {
       if (!result) // find the first occurrence
         result = true;
       else
@@ -58,38 +70,32 @@ bool DSFVector::isSingleton(const Label& label) const {
 }
 
 /* ************************************************************************* */
-std::set<size_t> DSFVector::set(const Label& label) const {
-  std::set<size_t> set;
-  V::const_iterator it = keys_.begin();
-  for (; it != keys_.end(); it++) {
-    if (findSet(*it) == label)
-      set.insert(*it);
-  }
+std::set<size_t> DSFVector::set(const size_t& label) const {
+  std::set < size_t > set;
+  BOOST_FOREACH(size_t key,keys_)
+    if (findSet(key) == label)
+      set.insert(key);
   return set;
 }
 
 /* ************************************************************************* */
-std::map<DSFVector::Label, std::set<size_t> > DSFVector::sets() const {
-  std::map<Label, std::set<size_t> > sets;
-  V::const_iterator it = keys_.begin();
-  for (; it != keys_.end(); it++) {
-    sets[findSet(*it)].insert(*it);
-  }
+std::map<size_t, std::set<size_t> > DSFVector::sets() const {
+  std::map<size_t, std::set<size_t> > sets;
+  BOOST_FOREACH(size_t key,keys_)
+    sets[findSet(key)].insert(key);
   return sets;
 }
 
 /* ************************************************************************* */
-std::map<DSFVector::Label, std::vector<size_t> > DSFVector::arrays() const {
-  std::map<Label, std::vector<size_t> > arrays;
-  V::const_iterator it = keys_.begin();
-  for (; it != keys_.end(); it++) {
-    arrays[findSet(*it)].push_back(*it);
-  }
+std::map<size_t, std::vector<size_t> > DSFVector::arrays() const {
+  std::map<size_t, std::vector<size_t> > arrays;
+  BOOST_FOREACH(size_t key,keys_)
+    arrays[findSet(key)].push_back(key);
   return arrays;
 }
 
 /* ************************************************************************* */
-void DSFVector::makeUnionInPlace(const size_t& i1, const size_t& i2)  {
+void DSFVector::makeUnionInPlace(const size_t& i1, const size_t& i2) {
   (*v_)[findSet(i2)] = findSet(i1);
 }
 
diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h
index dd22d5b83..f3040af23 100644
--- a/gtsam/base/DSFVector.h
+++ b/gtsam/base/DSFVector.h
@@ -18,64 +18,62 @@
 
 #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 {
+/**
+ * A fast implementation of disjoint set forests that uses vector as underly data structure.
+ * @addtogroup base
+ */
+class GTSAM_EXPORT DSFVector {
 
-  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
+public:
+  typedef std::vector<size_t> V; ///< Vector of ints
 
-  private:
-     // TODO could use existing memory to improve the efficiency
-    boost::shared_ptr<V> v_;
-    std::vector<size_t> keys_;
+private:
+  boost::shared_ptr<V> v_;///< Stores parent pointers, representative iff v[i]==i
+  std::vector<size_t> keys_;///< stores keys
 
-  public:
-    /// constructor that allocate a new memory
-    DSFVector(const size_t numNodes);
+public:
+  /// constructor that allocate new memory, uses sequential keys 0...numNodes-1
+  DSFVector(const size_t numNodes);
 
-    /// constructor that uses the existing memory
-    DSFVector(const boost::shared_ptr<V>& v_in, const std::vector<size_t>& keys);
+  /// constructor that allocates memory, uses given keys
+  DSFVector(const std::vector<size_t>& keys);
 
-    /// find the label of the set in which {key} lives
-    inline Label findSet(size_t key) const {
-      size_t parent = (*v_)[key];
-      while (parent != key) {
-        key = parent;
-        parent = (*v_)[key];
-      }
-      return parent;
+  /// 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 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];
     }
+    return parent;
+  }
 
-    /// find whether there is one and only one occurrence for the given {label}
-    bool isSingleton(const Label& label) const;
+  /// find whether there is one and only one occurrence for the given {label}
+  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;
+  /// get the nodes in the tree with the given label
+  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;
+  /// return all sets, i.e. a partition of all elements
+  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;
+  /// return all sets, i.e. a partition of all elements
+  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);
-
-  };
+  /// the in-place version of makeUnion
+  void makeUnionInPlace(const size_t& i1, const size_t& i2);
+};
 
 }
diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp
index cad2f6ded..9288e2aad 100644
--- a/gtsam/base/tests/testDSFVector.cpp
+++ b/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));
-  CHECK(!dsf.isSingleton(1));
-  CHECK( dsf.isSingleton(2));
+  EXPECT(!dsf.isSingleton(0));
+  EXPECT(!dsf.isSingleton(1));
+  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);}
 /* ************************************************************************* */