diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp index 929835cfa..24ac9b9e1 100644 --- a/gtsam/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -26,33 +26,55 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -DSFVector::DSFVector(const size_t numNodes) { +DSFBase::DSFBase(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); +} + +/* ************************************************************************* */ +DSFBase::DSFBase(const boost::shared_ptr& v_in) { + v_ = v_in; + int index = 0; + for (V::iterator it = v_->begin(); it != v_->end(); it++, index++) + *it = index; +} + +/* ************************************************************************* */ +size_t DSFBase::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; +} + +/* ************************************************************************* */ +void DSFBase::makeUnionInPlace(const size_t& i1, const size_t& i2) { + (*v_)[findSet(i2)] = findSet(i1); +} + +/* ************************************************************************* */ +DSFVector::DSFVector(const size_t numNodes) : + DSFBase(numNodes) { + keys_.reserve(numNodes); + for (size_t index = 0; index < numNodes; index++) + keys_.push_back(index); } /* ************************************************************************* */ 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; + DSFBase(1 + *std::max_element(keys.begin(), keys.end())), keys_(keys) { } /* ************************************************************************* */ DSFVector::DSFVector(const boost::shared_ptr& v_in, const std::vector& keys) : - keys_(keys) { + DSFBase(v_in), keys_(keys) { assert(*(std::max_element(keys.begin(), keys.end()))size()); - v_ = v_in; - BOOST_FOREACH(const size_t key, keys) - (*v_)[key] = key; } /* ************************************************************************* */ @@ -94,10 +116,5 @@ std::map > DSFVector::arrays() const { return arrays; } -/* ************************************************************************* */ -void DSFVector::makeUnionInPlace(const size_t& i1, const size_t& i2) { - (*v_)[findSet(i2)] = findSet(i1); -} - } // namespace gtsam diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h index f3040af23..9b7b9f66e 100644 --- a/gtsam/base/DSFVector.h +++ b/gtsam/base/DSFVector.h @@ -28,16 +28,39 @@ namespace gtsam { /** * A fast implementation of disjoint set forests that uses vector as underly data structure. + * This is the absolute minimal DSF data structure, and only allows size_t keys * @addtogroup base */ -class GTSAM_EXPORT DSFVector { +class GTSAM_EXPORT DSFBase { public: typedef std::vector V; ///< Vector of ints private: boost::shared_ptr v_;///< Stores parent pointers, representative iff v[i]==i - std::vector keys_;///< stores keys + +public: + /// constructor that allocate new memory, allows for keys 0...numNodes-1 + DSFBase(const size_t numNodes); + + /// constructor that uses the existing memory + DSFBase(const boost::shared_ptr& v_in); + + /// find the label of the set in which {key} lives + size_t findSet(size_t key) const; + + /// the in-place version of makeUnion + void makeUnionInPlace(const size_t& i1, const size_t& i2); +}; + +/** + * A fast implementation of disjoint set forests that uses vector as underly data structure. + * @addtogroup base + */ +class GTSAM_EXPORT DSFVector: public DSFBase { + +private: + std::vector keys_;///< stores keys to support more expensive operations public: /// constructor that allocate new memory, uses sequential keys 0...numNodes-1 @@ -49,17 +72,6 @@ public: /// 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 size_t& label) const; @@ -71,9 +83,6 @@ public: /// 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); }; } diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index 9288e2aad..cbd17a088 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -186,9 +186,6 @@ TEST(DSFVector, mergePairwiseMatches) { 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)); @@ -199,16 +196,13 @@ TEST(DSFVector, mergePairwiseMatches) { 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); }