From 0dbd016ca9ed320e05fb1361b75d45d43f98e1cb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2013 15:02:30 +0000 Subject: [PATCH] Cleaned up DSFVector and added pairwise matches example --- .cproject | 8 +++ gtsam/base/DSFVector.cpp | 64 ++++++++++++---------- gtsam/base/DSFVector.h | 86 +++++++++++++++--------------- gtsam/base/tests/testDSFVector.cpp | 85 ++++++++++++++++++++++------- 4 files changed, 151 insertions(+), 92 deletions(-) diff --git a/.cproject b/.cproject index fed8aa1f5..46d6f3844 100644 --- a/.cproject +++ b/.cproject @@ -1787,6 +1787,14 @@ true true + + make + -j5 + testDSFVector.run + true + true + true + make -j5 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 #include #include -#include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -DSFVector::DSFVector (const size_t numNodes) { - v_ = boost::make_shared(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_in, const std::vector& keys) : keys_(keys) { +DSFVector::DSFVector(const std::vector& 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_in, + const std::vector& keys) : + keys_(keys) { + assert(*(std::max_element(keys.begin(), keys.end()))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 DSFVector::set(const Label& label) const { - std::set set; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - if (findSet(*it) == label) - set.insert(*it); - } +std::set 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::sets() const { - std::map > sets; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - sets[findSet(*it)].insert(*it); - } +std::map > DSFVector::sets() const { + std::map > sets; + BOOST_FOREACH(size_t key,keys_) + sets[findSet(key)].insert(key); return sets; } /* ************************************************************************* */ -std::map > DSFVector::arrays() const { - std::map > arrays; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - arrays[findSet(*it)].push_back(*it); - } +std::map > DSFVector::arrays() const { + std::map > 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 -#include -#include -#include - #include +#include +#include +#include +#include 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 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 V; ///< Vector of ints - private: - // TODO could use existing memory to improve the efficiency - boost::shared_ptr v_; - std::vector keys_; +private: + boost::shared_ptr v_;///< Stores parent pointers, representative iff v[i]==i + std::vector 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_in, const std::vector& keys); + /// constructor that allocates memory, uses given keys + DSFVector(const std::vector& 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_in, const std::vector& 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 set(const Label& label) const; + /// get the nodes in the tree with the given label + std::set set(const size_t& label) const; - /// return all sets, i.e. a partition of all elements - std::map > sets() const; + /// return all sets, i.e. a partition of all elements + std::map > sets() const; - /// return all sets, i.e. a partition of all elements - std::map > arrays() const; + /// return all sets, i.e. a partition of all elements + std::map > 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 +#include + +#include + +#include #include #include #include #include using namespace boost::assign; -#include -#include +#include 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 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 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 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 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 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 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 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 expected; expected += 0, 1; - CHECK(expected == set); + EXPECT(expected == set); } /* ************************************************************************* */ @@ -154,17 +157,61 @@ TEST(DSFVector, set2) { LONGS_EQUAL(3, set.size()); std::set 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 keys; + keys += 1,2,3,4,5,6; + + // Create some "matches" + typedef pair Match; + vector 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 > 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 > sets = dsf.sets(); + LONGS_EQUAL(2, sets.size()); + set expected1; expected1 += 1,2,3; + set actual1 = sets[rep1]; + BOOST_FOREACH(size_t i, actual1) cout << i << " "; + EXPECT(expected1 == actual1); + set expected2; expected2 += 4,5,6; + set actual2 = sets[rep2]; + BOOST_FOREACH(size_t i, actual2) cout << i << " "; + EXPECT(expected2 == actual2); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */