diff --git a/.cproject b/.cproject
index 55ad7b743..8fc54f11b 100644
--- a/.cproject
+++ b/.cproject
@@ -317,6 +317,7 @@
make
+
clean
true
true
@@ -476,7 +477,6 @@
make
-
testBayesTree.run
true
false
@@ -484,6 +484,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -491,7 +492,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -683,7 +683,6 @@
make
-
testGraph.run
true
false
@@ -739,6 +738,7 @@
make
+
testSimulated2D.run
true
false
@@ -786,7 +786,6 @@
make
-
testErrors.run
true
false
@@ -794,12 +793,19 @@
make
-
testDSF.run
true
true
true
+
+make
+-j2
+testFactorGraph.run
+true
+true
+true
+
make
-j2
diff --git a/cpp/DSF.h b/cpp/DSF.h
index 7d8c2e9f1..c66ba509b 100644
--- a/cpp/DSF.h
+++ b/cpp/DSF.h
@@ -37,16 +37,19 @@ namespace gtsam {
// constructor
DSF(const Tree& tree) : Tree(tree) {}
+ // constructor with a list of unconnected keys
+ DSF(const std::list& 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 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;
}
}
diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h
index 1fca15477..93069b319 100644
--- a/cpp/FactorGraph-inl.h
+++ b/cpp/FactorGraph-inl.h
@@ -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; \
@@ -359,6 +360,37 @@ void FactorGraph::split(const PredecessorMap& tree, FactorGraph
+std::pair, FactorGraph > FactorGraph::splitMinimumSpanningTree() const {
+ // create an empty factor graph T (tree) and factor graph C (constraints)
+ FactorGraph T;
+ FactorGraph C;
+ DSF dsf(keys());
+
+ // while G is nonempty and T is not yet spanning
+ size_t m = nrFactors();
+ for (size_t i=0;i 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::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 combine(const FactorGraph& fg1, const FactorGraph findAndRemoveFactors(const Symbol& key);
/**
- * find the minimum spanning tree.
+ * find the minimum spanning tree using boost graph library
*/
template PredecessorMap findMinimumSpanningTree() const;
@@ -129,6 +129,11 @@ namespace gtsam {
template void split(const PredecessorMap& tree,
FactorGraph& Ab1, FactorGraph& Ab2) const;
+ /**
+ * find the minimum spanning tree using DSF
+ */
+ std::pair, FactorGraph > splitMinimumSpanningTree() const;
+
/**
* Check consistency of the index map, useful for debugging
*/
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index 82614367f..cb82a0a00 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -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
diff --git a/cpp/testDSF.cpp b/cpp/testDSF.cpp
index dc5ba300b..fd0551394 100644
--- a/cpp/testDSF.cpp
+++ b/cpp/testDSF.cpp
@@ -13,6 +13,7 @@ using namespace boost::assign;
#include
#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 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);}
/* ************************************************************************* */
diff --git a/cpp/testFactorgraph.cpp b/cpp/testFactorgraph.cpp
index 5059cfd94..f386e84d5 100644
--- a/cpp/testFactorgraph.cpp
+++ b/cpp/testFactorgraph.cpp
@@ -19,31 +19,6 @@ using namespace gtsam;
typedef boost::shared_ptr 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 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;ipush_back(f);
- else // otherwise add that factor to C
- C->push_back(f);
-}
- return std::pair(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));
}
/* ************************************************************************* */