diff --git a/cpp/testFactorgraph.cpp b/cpp/testFactorgraph.cpp index 6e00c1a57..5059cfd94 100644 --- a/cpp/testFactorgraph.cpp +++ b/cpp/testFactorgraph.cpp @@ -7,11 +7,71 @@ /*STL/C++*/ #include #include -using namespace std; - +#include +#include #include -#include "smallExample.h" + +#define GTSAM_MAGIC_KEY +#include "SymbolicFactorGraph.h" + +using namespace std; +using namespace gtsam; + +typedef boost::shared_ptr shared; /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +// 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 ) +{ + SymbolicFactorGraph G; + G.push_factor("x1", "x2"); + G.push_factor("x1", "x3"); + G.push_factor("x1", "x4"); + G.push_factor("x2", "x3"); + G.push_factor("x2", "x4"); + G.push_factor("x3", "x4"); + + shared T, C; + boost::tie(T, C) = splitMinimumSpanningTree(G); + + 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)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */