diff --git a/.cproject b/.cproject
index 41cddbb6b..b06ac7142 100644
--- a/.cproject
+++ b/.cproject
@@ -2,712 +2,712 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-make
-install
-true
-true
-true
-
-
-make
-check
-true
-true
-true
-
-
-make
--k
-check
-true
-false
-true
-
-
-make
-testSimpleCamera.run
-true
-true
-true
-
-
-make
--f local.mk
-testCal3_S2.run
-true
-true
-true
-
-
-make
-
-testVSLAMFactor.run
-true
-true
-true
-
-
-make
-testCalibratedCamera.run
-true
-true
-true
-
-
-make
-
-testGaussianConditional.run
-true
-true
-true
-
-
-make
-testPose2.run
-true
-true
-true
-
-
-make
-testRot3.run
-true
-true
-true
-
-
-make
-
-testNonlinearOptimizer.run
-true
-true
-true
-
-
-make
-testGaussianFactor.run
-true
-true
-true
-
-
-make
-testGaussianFactorGraph.run
-true
-true
-true
-
-
-make
-
-testNonlinearFactorGraph.run
-true
-true
-true
-
-
-make
-testPose3.run
-true
-true
-true
-
-
-make
-
-testVectorConfig.run
-true
-true
-true
-
-
-make
-
-testPoint2.run
-true
-true
-true
-
-
-make
-testNonlinearFactor.run
-true
-true
-true
-
-
-make
-timeGaussianFactor.run
-true
-true
-true
-
-
-make
-timeGaussianFactorGraph.run
-true
-true
-true
-
-
-make
-testGaussianBayesNet.run
-true
-true
-true
-
-
-make
-
-testBayesTree.run
-true
-false
-true
-
-
-make
-testSymbolicBayesNet.run
-true
-false
-true
-
-
-make
-
-testSymbolicFactorGraph.run
-true
-false
-true
-
-
-make
-testVector.run
-true
-true
-true
-
-
-make
-testMatrix.run
-true
-true
-true
-
-
-make
-
-testVSLAMGraph.run
-true
-true
-true
-
-
-make
-
-testNonlinearEquality.run
-true
-true
-true
-
-
-make
-testSQP.run
-true
-true
-true
-
-
-make
-
-testNonlinearConstraint.run
-true
-true
-true
-
-
-make
-testSQPOptimizer.run
-true
-true
-true
-
-
-make
-testVSLAMConfig.run
-true
-true
-true
-
-
-make
-testControlConfig.run
-true
-true
-true
-
-
-make
-testControlPoint.run
-true
-true
-true
-
-
-make
-
-testControlGraph.run
-true
-true
-true
-
-
-make
-testOrdering.run
-true
-true
-true
-
-
-make
-
-testPose2Constraint.run
-true
-true
-true
-
-
-make
-testRot2.run
-true
-true
-true
-
-
-make
-testGaussianBayesTree.run
-true
-true
-true
-
-
-make
-
-testPose3Factor.run
-true
-true
-true
-
-
-make
-
-testUrbanGraph.run
-true
-true
-true
-
-
-make
-
-testUrbanConfig.run
-true
-true
-true
-
-
-make
-
-testUrbanMeasurement.run
-true
-true
-true
-
-
-make
-testUrbanOdometry.run
-true
-true
-true
-
-
-make
-
-testIterative.run
-true
-true
-true
-
-
-make
-
-testGaussianISAM2.run
-true
-true
-true
-
-
-make
-testSubgraphPreconditioner.run
-true
-true
-true
-
-
-make
-testBayesNetPreconditioner.run
-true
-true
-true
-
-
-make
-
-testPose2Factor.run
-true
-true
-true
-
-
-make
-
-timeRot3.run
-true
-true
-true
-
-
-make
-
-install
-true
-true
-true
-
-
-make
-
-install
-true
-true
-true
-
-
-make
-
-clean
-true
-true
-true
-
-
-make
-
-check
-true
-true
-true
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ make
+ install
+ true
+ true
+ true
+
+
+ make
+ check
+ true
+ true
+ true
+
+
+ make
+ -k
+ check
+ true
+ false
+ true
+
+
+ make
+ testSimpleCamera.run
+ true
+ true
+ true
+
+
+ make
+ -f local.mk
+ testCal3_S2.run
+ true
+ true
+ true
+
+
+ make
+
+ testVSLAMFactor.run
+ true
+ true
+ true
+
+
+ make
+ testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+
+ testGaussianConditional.run
+ true
+ true
+ true
+
+
+ make
+ testPose2.run
+ true
+ true
+ true
+
+
+ make
+ testRot3.run
+ true
+ true
+ true
+
+
+ make
+
+ testNonlinearOptimizer.run
+ true
+ true
+ true
+
+
+ make
+ testGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ testGaussianFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+
+ testNonlinearFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+ testPose3.run
+ true
+ true
+ true
+
+
+ make
+
+ testVectorConfig.run
+ true
+ true
+ true
+
+
+ make
+
+ testPoint2.run
+ true
+ true
+ true
+
+
+ make
+ testNonlinearFactor.run
+ true
+ true
+ true
+
+
+ make
+ timeGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ timeGaussianFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+ testGaussianBayesNet.run
+ true
+ true
+ true
+
+
+ make
+
+ testBayesTree.run
+ true
+ false
+ true
+
+
+ make
+ testSymbolicBayesNet.run
+ true
+ false
+ true
+
+
+ make
+
+ testSymbolicFactorGraph.run
+ true
+ false
+ true
+
+
+ make
+ testVector.run
+ true
+ true
+ true
+
+
+ make
+ testMatrix.run
+ true
+ true
+ true
+
+
+ make
+
+ testVSLAMGraph.run
+ true
+ true
+ true
+
+
+ make
+
+ testNonlinearEquality.run
+ true
+ true
+ true
+
+
+ make
+ testSQP.run
+ true
+ true
+ true
+
+
+ make
+
+ testNonlinearConstraint.run
+ true
+ true
+ true
+
+
+ make
+ testSQPOptimizer.run
+ true
+ true
+ true
+
+
+ make
+ testVSLAMConfig.run
+ true
+ true
+ true
+
+
+ make
+ testControlConfig.run
+ true
+ true
+ true
+
+
+ make
+ testControlPoint.run
+ true
+ true
+ true
+
+
+ make
+
+ testControlGraph.run
+ true
+ true
+ true
+
+
+ make
+ testOrdering.run
+ true
+ true
+ true
+
+
+ make
+
+ testPose2Constraint.run
+ true
+ true
+ true
+
+
+ make
+ testRot2.run
+ true
+ true
+ true
+
+
+ make
+ testGaussianBayesTree.run
+ true
+ true
+ true
+
+
+ make
+
+ testPose3Factor.run
+ true
+ true
+ true
+
+
+ make
+
+ testUrbanGraph.run
+ true
+ true
+ true
+
+
+ make
+
+ testUrbanConfig.run
+ true
+ true
+ true
+
+
+ make
+
+ testUrbanMeasurement.run
+ true
+ true
+ true
+
+
+ make
+ testUrbanOdometry.run
+ true
+ true
+ true
+
+
+ make
+
+ testIterative.run
+ true
+ true
+ true
+
+
+ make
+
+ testGaussianISAM2.run
+ true
+ true
+ true
+
+
+ make
+ testSubgraphPreconditioner.run
+ true
+ true
+ true
+
+
+ make
+ testBayesNetPreconditioner.run
+ true
+ true
+ true
+
+
+ make
+
+ testPose2Factor.run
+ true
+ true
+ true
+
+
+ make
+
+ timeRot3.run
+ true
+ true
+ true
+
+
+ make
+
+ install
+ true
+ true
+ true
+
+
+ make
+
+ install
+ true
+ true
+ true
+
+
+ make
+
+ clean
+ true
+ true
+ true
+
+
+ make
+
+ check
+ true
+ true
+ true
+
+
+
+
+
+
+
+
diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h
index 3f6217e08..49f7c78da 100644
--- a/cpp/FactorGraph-inl.h
+++ b/cpp/FactorGraph-inl.h
@@ -18,10 +18,16 @@
#include
#include
#include
+#include
+#include
+#include
#include
#include "Ordering.h"
#include "FactorGraph.h"
+
+
+
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
@@ -268,6 +274,63 @@ void FactorGraph::associateFactor(int index, sharedFactor factor) {
}
}
+/* ************************************************************************* */
+template
+vector > FactorGraph::findMinimumSpanningTree() const {
+
+ typedef boost::adjacency_list<
+ boost::vecS, boost::vecS, boost::undirectedS,
+ boost::property,
+ boost::property > Graph;
+ typedef boost::graph_traits::vertex_descriptor Vertex;
+ typedef boost::graph_traits::edge_descriptor Edge;
+
+ // convert the factor graph to boost graph
+ Graph g(0);
+ map key2vertex;
+ Vertex v1, v2;
+ BOOST_FOREACH(sharedFactor factor, factors_){
+ if (factor->keys().size() > 2)
+ throw(invalid_argument("findMinimumSpanningTree: only support factors with two keys"));
+
+ if (factor->keys().size() == 1)
+ continue;
+
+ string key1 = factor->keys().front();
+ string key2 = factor->keys().back();
+
+ if (key2vertex.find(key1) == key2vertex.end()) {
+ v1 = add_vertex(key1, g);
+ key2vertex.insert(make_pair(key1, v1));
+ } else
+ v1 = key2vertex[key1];
+
+ if (key2vertex.find(key2) == key2vertex.end()) {
+ v2 = add_vertex(key2, g);
+ key2vertex.insert(make_pair(key2, v2));
+ } else
+ v2 = key2vertex[key2];
+
+ boost::property edge_property(1); // assume constant edge weight here
+ boost::add_edge(v1, v2, edge_property, g);
+ }
+
+ // find minimum spanning tree
+ vector spanning_tree;
+ boost::kruskal_minimum_spanning_tree(g, back_inserter(spanning_tree));
+
+ // convert edge to skin
+ vector > tree;
+ for (vector::iterator ei = spanning_tree.begin(); ei
+ != spanning_tree.end(); ++ei) {
+ string key1 = boost::get(boost::vertex_name, g, boost::source(*ei,g));
+ string key2 = boost::get(boost::vertex_name, g, boost::target(*ei,g));
+ tree.push_back(make_pair(key1, key2));
+ }
+
+ return tree;
+}
+
/* ************************************************************************* */
/* find factors and remove them from the factor graph: O(n) */
/* ************************************************************************* */
diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h
index b1c4a6fa5..223ca9711 100644
--- a/cpp/FactorGraph.h
+++ b/cpp/FactorGraph.h
@@ -116,6 +116,11 @@ namespace gtsam {
*/
std::vector findAndRemoveFactors(const std::string& key);
+ /**
+ * find the minimum spanning tree
+ */
+ std::vector > findMinimumSpanningTree() const;
+
private:
/** Associate factor index with the variables connected to the factor */
void associateFactor(int index, sharedFactor factor);
diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp
index 0d4c09f78..d9a42c647 100644
--- a/cpp/GaussianFactorGraph.cpp
+++ b/cpp/GaussianFactorGraph.cpp
@@ -319,4 +319,3 @@ boost::shared_ptr GaussianFactorGraph::conjugateGradientDescent_(
}
/* ************************************************************************* */
-
diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h
index 9f99ee201..093eec54e 100644
--- a/cpp/GaussianFactorGraph.h
+++ b/cpp/GaussianFactorGraph.h
@@ -218,6 +218,7 @@ namespace gtsam {
boost::shared_ptr conjugateGradientDescent_(
const VectorConfig& x0, bool verbose = false, double epsilon = 1e-3,
size_t maxIterations = 0) const;
+
};
}
diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp
index c6fd8c23c..7825682ee 100644
--- a/cpp/testGaussianFactorGraph.cpp
+++ b/cpp/testGaussianFactorGraph.cpp
@@ -730,6 +730,23 @@ TEST( GaussianFactorGraph, constrained_multi2 )
CHECK(assert_equal(expected, actual));
}
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, findMinimumSpanningTree )
+{
+ GaussianFactorGraph g;
+ Matrix I = eye(2);
+ Vector b = Vector_(0, 0, 0);
+ g.add("x1", I, "x2", I, b, 0);
+ g.add("x1", I, "x3", I, b, 0);
+ g.add("x1", I, "x4", I, b, 0);
+ g.add("x2", I, "x3", I, b, 0);
+ g.add("x2", I, "x4", I, b, 0);
+ g.add("x3", I, "x4", I, b, 0);
+
+ vector > tree = g.findMinimumSpanningTree();
+ LONGS_EQUAL(3,tree.size());
+}
+
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */