diff --git a/.cproject b/.cproject index 46d6f3844..8d2bd0378 100644 --- a/.cproject +++ b/.cproject @@ -487,6 +487,14 @@ true true + + make + -j5 + testDSFMap.run + true + true + true + make -j2 diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index 166af9df5..d900e1779 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/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)); diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h index 909566ff1..a827a2b90 100644 --- a/gtsam_unstable/base/DSF.h +++ b/gtsam_unstable/base/DSF.h @@ -18,12 +18,12 @@ #pragma once +#include +#include #include #include #include #include -#include -#include namespace gtsam { @@ -40,11 +40,10 @@ namespace gtsam { class DSF : protected BTree { public: - typedef KEY Label; // label can be different from key, but for now they are same typedef DSF Self; typedef std::set Set; - typedef BTree Tree; - typedef std::pair KeyLabel; + typedef BTree Tree; + typedef std::pair 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 sets() const { - std::map sets; + std::map sets() const { + std::map 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 partition(const std::list& keys) const { - std::map partitions; + std::map partition(const std::list& keys) const { + std::map 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) diff --git a/gtsam_unstable/base/DSFMap.h b/gtsam_unstable/base/DSFMap.h new file mode 100644 index 000000000..14b69f8c7 --- /dev/null +++ b/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 + +namespace gtsam { + +/** + * Disjoint set forest using an STL map data structure underneath + * Uses rank compression but not union by rank :-( + * @addtogroup base + */ +template +class DSFMap { + + /// We store the forest in an STL map + typedef std::map 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); + } + +}; + +} diff --git a/gtsam_unstable/base/tests/testDSFMap.cpp b/gtsam_unstable/base/tests/testDSFMap.cpp new file mode 100644 index 000000000..121bd50d0 --- /dev/null +++ b/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 + +#include +#include +//#include +using namespace boost::assign; +// +#include +// +//#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(DSFMap, find) { + DSFMap 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 dsf; + dsf.merge(0,2); + EXPECT(dsf.find(0) == dsf.find(2)); +} +/* ************************************************************************* */ +TEST(DSFMap, merge2) { + DSFMap dsf; + dsf.merge(2,0); + EXPECT(dsf.find(0) == dsf.find(2)); +} + +/* ************************************************************************* */ +TEST(DSFMap, merge3) { + DSFMap dsf; + dsf.merge(0,1); + dsf.merge(1,2); + EXPECT(dsf.find(0) == dsf.find(2)); +} + +/* ************************************************************************* */ +TEST(DSFMap, mergePairwiseMatches) { + + // Create some "matches" + typedef pair Match; + list matches; + matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); + + // Merge matches + DSFMap 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 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 measurements; + measurements += m11,m12,m14, m22,m23,m25,m26; + + // Create some "matches" + typedef pair Match; + list matches; + matches += Match(m11,m22), Match(m12,m23), Match(m14,m25), Match(m14,m26); + + // Merge matches + DSFMap 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);} +/* ************************************************************************* */ +