Cleaned up DSFVector and added pairwise matches example
Frank Dellaert
parent
17a74da786
commit
0dbd016ca9
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue