12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added splitMinimumSpanningTree that uses DSF

release/4.3a0
Kai Ni 2010-03-30 07:27:10 +00:00
parent cc2e42aa15
commit a647c84c16
7 changed files with 103 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -317,6 +317,7 @@
</target>
<target name="clean" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -476,7 +477,6 @@
</target>
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -484,6 +484,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -491,7 +492,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -683,7 +683,6 @@
</target>
<target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -739,6 +738,7 @@
</target>
<target name="testSimulated2D.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -786,7 +786,6 @@
</target>
<target name="testErrors.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -794,12 +793,19 @@
</target>
<target name="testDSF.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

24
cpp/DSF.h
View File

@ -37,16 +37,19 @@ namespace gtsam {
// constructor
DSF(const Tree& tree) : Tree(tree) {}
// constructor with a list of unconnected keys
DSF(const std::list<Key>& keys) : Tree() { BOOST_FOREACH(const Key& key, keys) *this = this->add(key, key); }
// create a new singleton, does nothing if already exists
Self makeSet(const Key& key) const { if (mem(key)) return *this; else return add(key, key); }
Self makeSet(const Key& key) const { if (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 parent = find(key);
Key parent = this->find(key);
return parent == key ? key : findSet(parent); }
// return a new DSF where x and y are in the same set. Kai: the caml implementation is not const, and I followed
Self makeUnion(const Key& key1, const Key& key2) { return add(findSet_(key2), findSet_(key1)); }
Self makeUnion(const Key& key1, const Key& key2) { return this->add(findSet_(key2), findSet_(key1)); }
// create a new singleton with two connected keys
Self makePair(const Key& key1, const Key& key2) const { return makeSet(key1).makeSet(key2).makeUnion(key1, key2); }
@ -71,7 +74,7 @@ namespace gtsam {
DSF map(boost::function<Key(const Key&)> func) const {
DSF t;
BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
t.add(func(pair.first), func(pair.second));
t = t.add(func(pair.first), func(pair.second));
return t;
}
@ -105,8 +108,15 @@ namespace gtsam {
/** equality */
bool operator==(const Self& t) const { return (Tree)*this == (Tree)t; }
/** inequality */
bool operator!=(const Self& t) const { return (Tree)*this != (Tree)t; }
// print the object
void print(std::string& name = "DSF") const { Tree::print(name); }
void print(const std::string& name = "DSF") const {
std::cout << name << std::endl;
BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
std::cout << (std::string)pair.first << " " << (std::string)pair.second << std::endl;
}
private:
@ -115,12 +125,12 @@ namespace gtsam {
* the root, each parent pointer is made to directly point to it
*/
Key findSet_(const Key& key) {
Key parent = find(key);
Key parent = this->find(key);
if (parent == key)
return parent;
else {
Key label = findSet_(parent);
*this = add(key, label);
*this = this->add(key, label);
return label;
}
}

35
cpp/FactorGraph-inl.h
View File

@ -23,6 +23,7 @@
#include "Ordering.h"
#include "FactorGraph.h"
#include "graph-inl.h"
#include "DSF.h"
#define INSTANTIATE_FACTOR_GRAPH(F) \
template class FactorGraph<F>; \
@ -359,6 +360,37 @@ void FactorGraph<Factor>::split(const PredecessorMap<Key>& tree, FactorGraph<Fac
}
}
/* ************************************************************************* */
template<class Factor>
std::pair<FactorGraph<Factor>, FactorGraph<Factor> > FactorGraph<Factor>::splitMinimumSpanningTree() const {
// create an empty factor graph T (tree) and factor graph C (constraints)
FactorGraph<Factor> T;
FactorGraph<Factor> C;
DSF<Symbol> dsf(keys());
// while G is nonempty and T is not yet spanning
size_t m = nrFactors();
for (size_t i=0;i<m;i++) {
const sharedFactor& f = factors_[i];
// retrieve the labels of all the keys
set<Symbol> labels;
BOOST_FOREACH(const Symbol& key, f->keys())
labels.insert(dsf.findSet(key));
// if that factor connects two different trees, then add it to T
if (labels.size() > 1) {
T.push_back(f);
set<Symbol>::const_iterator it = labels.begin();
Symbol root = *it;
for (it++; it!=labels.end(); it++)
dsf = dsf.makeUnion(root, *it);
} else // otherwise add that factor to C
C.push_back(f);
}
return make_pair(T,C);
}
/* ************************************************************************* */
/* find factors and remove them from the factor graph: O(n) */
/* ************************************************************************* */
@ -381,4 +413,5 @@ FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Fa
return fg;
}
}
} // namespace gtsam

7
cpp/FactorGraph.h
View File

@ -118,7 +118,7 @@ namespace gtsam {
std::vector<sharedFactor> findAndRemoveFactors(const Symbol& key);
/**
* find the minimum spanning tree.
* find the minimum spanning tree using boost graph library
*/
template<class Key, class Factor2> PredecessorMap<Key> findMinimumSpanningTree() const;
@ -129,6 +129,11 @@ namespace gtsam {
template<class Key, class Factor2> void split(const PredecessorMap<Key>& tree,
FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const;
/**
* find the minimum spanning tree using DSF
*/
std::pair<FactorGraph<Factor>, FactorGraph<Factor> > splitMinimumSpanningTree() const;
/**
* Check consistency of the index map, useful for debugging
*/

6
cpp/Makefile.am
View File

@ -70,13 +70,13 @@ headers += BayesTree.h BayesTree-inl.h
headers += ISAM.h ISAM-inl.h GaussianISAM.h
headers += ISAM2.h ISAM2-inl.h GaussianISAM2.h
sources += GaussianISAM.cpp GaussianISAM2.cpp
check_PROGRAMS += testGraph testFactorgraph testInference testOrdering
check_PROGRAMS += testGraph testFactorGraph testInference testOrdering
check_PROGRAMS += testBayesTree testISAM testGaussianISAM testGaussianISAM2
testGraph_SOURCES = testGraph.cpp
testGraph_LDADD = libgtsam.la
testFactorgraph_SOURCES = testFactorgraph.cpp
testFactorGraph_SOURCES = testFactorgraph.cpp
testInference_SOURCES = testInference.cpp
testFactorgraph_LDADD = libgtsam.la
testFactorGraph_LDADD = libgtsam.la
testInference_LDADD = libgtsam.la
testBayesTree_SOURCES = testBayesTree.cpp
testBayesTree_LDADD = libgtsam.la

29
cpp/testDSF.cpp
View File

@ -13,6 +13,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "DSF.h"
#include "key.h"
using namespace std;
using namespace gtsam;
@ -191,10 +192,10 @@ TEST(DSF, map) {
DSFInt actual = dsf.map(&func);
DSFInt expected;
dsf = dsf.makeSet(15);
dsf = dsf.makeSet(16);
dsf = dsf.makeSet(17);
dsf = dsf.makeUnion(15,16);
expected = expected.makeSet(15);
expected = expected.makeSet(16);
expected = expected.makeSet(17);
expected = expected.makeUnion(15,16);
CHECK(actual == expected);
}
@ -216,6 +217,26 @@ TEST(DSF, flatten) {
expected = expected.makePair(1, 7);
CHECK(actual == expected);
}
/* ************************************************************************* */
TEST(DSF, flatten2) {
Symbol x1('x',1);
Symbol x2('x',2), x3('x',3), x4('x',4);
list<Symbol> keys; keys += x1,x2,x3,x4;
DSFSymbol dsf(keys);
dsf = dsf.makeUnion(x1,x2);
dsf = dsf.makeUnion(x3,x4);
dsf = dsf.makeUnion(x1,x3);
CHECK(dsf != dsf.flatten());
DSFSymbol expected2;
expected2 = expected2.makePair(x1, x2);
expected2 = expected2.makePair(x1, x3);
expected2 = expected2.makePair(x1, x4);
CHECK(expected2 == dsf.flatten());
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

39
cpp/testFactorgraph.cpp
View File

@ -19,31 +19,6 @@ using namespace gtsam;
typedef boost::shared_ptr<SymbolicFactorGraph> shared;
/* ************************************************************************* */
// This function is not done yet
// It needs a test to check whether a factor is in the same component
// This can be done with a disjoint-set data structure (Union&Find) to keep
// track of which vertices are in which components.
std::pair<shared, shared> splitMinimumSpanningTree(SymbolicFactorGraph& G) {
// create an empty factor graph T (tree) and factor graph C (constraints)
shared T(new SymbolicFactorGraph);
shared C(new SymbolicFactorGraph);
// while G is nonempty and T is not yet spanning
size_t m = G.nrFactors();
for (size_t i=0;i<m;i++) {
// remove an factor with minimum weight from G
const SymbolicFactorGraph::sharedFactor& f = G[i];
G.remove(i);
// if that factor connects two different trees, then add it to T
if (true) // TODO: test whether f is in the same component
T->push_back(f);
else // otherwise add that factor to C
C->push_back(f);
}
return std::pair<shared, shared>(T,C);
}
/* ************************************************************************* */
TEST( FactorGraph, splitMinimumSpanningTree )
{
@ -55,18 +30,18 @@ TEST( FactorGraph, splitMinimumSpanningTree )
G.push_factor("x2", "x4");
G.push_factor("x3", "x4");
shared T, C;
boost::tie(T, C) = splitMinimumSpanningTree(G);
SymbolicFactorGraph T, C;
boost::tie(T, C) = G.splitMinimumSpanningTree();
SymbolicFactorGraph expectedT, expectedC;
expectedT.push_factor("x1", "x2");
expectedT.push_factor("x1", "x3");
expectedT.push_factor("x1", "x4");
expectedT.push_factor("x2", "x3");
expectedT.push_factor("x2", "x4");
expectedT.push_factor("x3", "x4");
CHECK(assert_equal(expectedT,*T));
CHECK(assert_equal(expectedC,*C));
expectedC.push_factor("x2", "x3");
expectedC.push_factor("x2", "x4");
expectedC.push_factor("x3", "x4");
CHECK(assert_equal(expectedT,T));
CHECK(assert_equal(expectedC,C));
}
/* ************************************************************************* */