Kruskal skeleton

cpp/testFactorgraph.cpp

@@ -7,11 +7,71 @@
 /*STL/C++*/
 #include <list>
 #include <iostream>
-using namespace std;
-
+#include <boost/shared_ptr.hpp>
+#include <boost/tuple/tuple.hpp>
 #include <CppUnitLite/TestHarness.h>
-#include "smallExample.h"
+
+#define GTSAM_MAGIC_KEY
+#include "SymbolicFactorGraph.h"
+
+using namespace std;
+using namespace gtsam;
+
+typedef boost::shared_ptr<SymbolicFactorGraph> 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<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 )
+{
+	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);
+}
 /* ************************************************************************* */