1) add a compose function that compose all the poses in a factor graph given the spanning tree

2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes

cpp/BetweenFactor.h

@@ -79,6 +79,9 @@ namespace gtsam {
 		inline const std::string& key1() const { return key1_;}
 		inline const std::string& key2() const { return key2_;}
 
+		/** return the measured */
+		inline const T measured() const {return measured_;}
+
 		/** keys as a list */
 		inline std::list<std::string> keys() const { return keys_;}
 

cpp/FactorGraph-inl.h

@@ -18,8 +18,6 @@
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/format.hpp>
-#include <boost/graph/graph_traits.hpp>
-#include <boost/graph/adjacency_list.hpp>
 #include <boost/graph/prim_minimum_spanning_tree.hpp>
 #include <colamd/colamd.h>
 #include "Ordering.h"
@@ -275,52 +273,16 @@ void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
 template<class Factor>
 map<string, string> FactorGraph<Factor>::findMinimumSpanningTree() const {
 
-	typedef boost::adjacency_list<
+	SDGraph g = gtsam::toBoostGraph<FactorGraph<Factor>, sharedFactor>(*this);
-		boost::vecS, boost::vecS, boost::undirectedS,
-		boost::property<boost::vertex_name_t, string>,
-		boost::property<boost::edge_weight_t, int> > Graph;
-	typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
-	typedef boost::graph_traits<Graph>::vertex_iterator VertexIterator;
-	typedef boost::graph_traits<Graph>::edge_descriptor Edge;
-
-	// convert the factor graph to boost graph
-	Graph g(0);
-	map<string, Vertex> key2vertex;
-	Vertex v1, v2;
-	BOOST_FOREACH(sharedFactor factor, factors_){
-		if (factor->keys().size() > 2)
-			throw(invalid_argument("findMinimumSpanningTree: only support factors with at most 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<boost::edge_weight_t, int> edge_property(1);  // assume constant edge weight here
-		boost::add_edge(v1, v2, edge_property, g);
-	}
 
 	// find minimum spanning tree
-	vector<Vertex> p_map(boost::num_vertices(g));
+	vector<BoostVertex> p_map(boost::num_vertices(g));
 	prim_minimum_spanning_tree(g, &p_map[0]);
 
 	// convert edge to string pairs
 	map<string, string> tree;
-	VertexIterator itVertex = boost::vertices(g).first;
+	BoostVertexIterator itVertex = boost::vertices(g).first;
-	for (vector<Vertex>::iterator vi = p_map.begin(); vi!=p_map.end(); itVertex++, vi++) {
+	for (vector<BoostVertex>::iterator vi = p_map.begin(); vi!=p_map.end(); itVertex++, vi++) {
 		string key = boost::get(boost::vertex_name, g, *itVertex);
 		string parent = boost::get(boost::vertex_name, g, *vi);
 		// printf("%s parent: %s\n", key.c_str(), parent.c_str());

cpp/FactorGraph.h

@@ -17,6 +17,7 @@
 
 #include "Testable.h"
 #include "BayesNet.h"
+#include "graph-inl.h"
 
 namespace gtsam {
 

cpp/Makefile.am

@@ -66,14 +66,17 @@ testSymbolicBayesNet_LDADD      = libgtsam.la
 
 # Inference
 headers += inference.h inference-inl.h
+headers += graph-inl.h
 headers += FactorGraph.h FactorGraph-inl.h
 headers += BayesNet.h BayesNet-inl.h
 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 += 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
 testInference_SOURCES          = $(example) testInference.cpp
 testFactorgraph_LDADD          = libgtsam.la

cpp/Ordering.cpp

@@ -9,65 +9,44 @@
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
-#include <boost/graph/adjacency_list.hpp>
-#include <boost/graph/breadth_first_search.hpp>
 
+#include "graph-inl.h"
 #include "Ordering.h"
 
+
 using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
 #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
 
-class key_visitor : public boost::default_bfs_visitor {
+class ordering_key_visitor : public boost::default_bfs_visitor {
 public:
-	key_visitor(Ordering& ordering_in) : ordering_(ordering_in) {}
+	ordering_key_visitor(Ordering& ordering_in) : ordering_(ordering_in) {}
-	template <typename Vertex, typename Graph> void discover_vertex(Vertex u, const Graph& g) const {
+	template <typename Vertex, typename Graph> void discover_vertex(Vertex v, const Graph& g) const {
-		string key = boost::get(boost::vertex_name, g, u);
+		string key = boost::get(boost::vertex_name, g, v);
 		ordering_.push_front(key);
 	}
 	Ordering& ordering_;
 };
 
+class ordering_edge_action {
+public:
+	void act (string& child, string& parent, SVertex& v1, SVertex& v2, SGraph& g){
+		//boost::add_edge(v1, v2, g);
+	}
+};
+
 /* ************************************************************************* */
 Ordering::Ordering(const map<string, string>& p_map) {
 
-	typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
+	SGraph g;
-		boost::property<boost::vertex_name_t, string> > Graph;
+	SVertex root;
-	typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
+	map<string, SVertex> key2vertex;
-
+	boost::tie(g, root, key2vertex) = predecessorMap2Graph<SGraph, SVertex>(p_map);
-	// build the graph corresponding to the predecessor map
-	Graph g(0);
-	map<string, Vertex> key2vertex;
-	Vertex v1, v2, root;
-	string child, parent;
-	bool foundRoot = false;
-	FOREACH_PAIR(child, parent, p_map) {
-		if (key2vertex.find(child) == key2vertex.end()) {
-			 v1 = add_vertex(child, g);
-			 key2vertex.insert(make_pair(child, v1));
-		 } else
-			 v1 = key2vertex[child];
-
-		if (key2vertex.find(parent) == key2vertex.end()) {
-			 v2 = add_vertex(parent, g);
-			 key2vertex.insert(make_pair(parent, v2));
-		 } else
-			 v2 = key2vertex[parent];
-
-		if (child.compare(parent) == 0) {
-			root = v1;
-			foundRoot = true;
-		} else
-			boost::add_edge(v1, v2, g);
-	}
-
-	if (!foundRoot)
-		throw invalid_argument("Ordering: invalid predecessor map!");
 
 	// breadth first visit on the graph
-	key_visitor vis(*this);
+	ordering_key_visitor vis(*this);
 	boost::breadth_first_search(g, root, boost::visitor(vis));
 }
 

cpp/Pose2Graph.cpp

@@ -3,11 +3,14 @@
  * @brief   A factor graph for the 2D PoseSLAM problem
  * @authors Frank Dellaert, Viorela Ila
  */
-//#include "NonlinearOptimizer-inl.h"
 #include "FactorGraph-inl.h"
 #include "NonlinearFactorGraph-inl.h"
+#include "graph-inl.h"
 #include "Pose2Graph.h"
 
+using namespace std;
+using namespace gtsam;
+
 namespace gtsam {
 
 // explicit instantiation so all the code is there and we can link with it
@@ -15,8 +18,12 @@ template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ;
 template class NonlinearFactorGraph<Pose2Config> ;
 //template class NonlinearOptimizer<Pose2Graph, Pose2Config> ;
 
+/* ************************************************************************* */
+
 bool Pose2Graph::equals(const Pose2Graph& p, double tol) const {
 	return false;
 }
 
+/* ************************************************************************* */
+
 } // namespace gtsam

cpp/graph-inl.h

@@ -0,0 +1,193 @@
+/*
+ * graph-inl.h
+ *
+ *   Created on: Jan 11, 2010
+ *       Author: nikai
+ *  Description: Graph algorithm using boost library
+ */
+
+#pragma once
+
+#include <boost/foreach.hpp>
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/breadth_first_search.hpp>
+
+using namespace std;
+
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+namespace gtsam {
+
+/* ************************************************************************* */
+/**
+ * type definitions
+ */
+typedef boost::adjacency_list<
+	boost::vecS, boost::vecS, boost::undirectedS,
+	boost::property<boost::vertex_name_t, std::string>,
+	boost::property<boost::edge_weight_t, double> > SDGraph;
+typedef boost::graph_traits<SDGraph>::vertex_descriptor BoostVertex;
+typedef boost::graph_traits<SDGraph>::vertex_iterator BoostVertexIterator;
+
+typedef boost::adjacency_list<
+	boost::vecS, boost::vecS, boost::directedS,
+	boost::property<boost::vertex_name_t, string> > SGraph;
+typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
+
+/* ************************************************************************* */
+/**
+ * Convert the factor graph to a boost undirected graph
+ */
+template<class G, class F>
+SDGraph toBoostGraph(const G& graph) {
+	// convert the factor graph to boost graph
+	SDGraph g(0);
+	map<string, BoostVertex> key2vertex;
+	BoostVertex v1, v2;
+	BOOST_FOREACH(F factor, graph) {
+		if (factor->keys().size() > 2)
+			throw(invalid_argument("toBoostGraph: only support factors with at most 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<boost::edge_weight_t, double> edge_property(1.0);  // assume constant edge weight here
+		boost::add_edge(v1, v2, edge_property, g);
+	}
+
+	return g;
+}
+
+/* ************************************************************************* */
+/**
+ *  build the graph corresponding to the predecessor map. Excute action for each edge.
+ */
+template<class G, class V>
+boost::tuple<G, V, map<string, V> > predecessorMap2Graph(const map<string, string>& p_map) {
+
+	G g(0);
+	map<string, V> key2vertex;
+	V v1, v2, root;
+	string child, parent;
+	bool foundRoot = false;
+	FOREACH_PAIR(child, parent, p_map) {
+		if (key2vertex.find(child) == key2vertex.end()) {
+			 v1 = add_vertex(child, g);
+			 key2vertex.insert(make_pair(child, v1));
+		 } else
+			 v1 = key2vertex[child];
+
+		if (key2vertex.find(parent) == key2vertex.end()) {
+			 v2 = add_vertex(parent, g);
+			 key2vertex.insert(make_pair(parent, v2));
+		 } else
+			 v2 = key2vertex[parent];
+
+		if (child.compare(parent) == 0) {
+			root = v1;
+			foundRoot = true;
+		} else
+			boost::add_edge(v2, v1, g); // edge is from parent to child
+	}
+
+	if (!foundRoot)
+		throw invalid_argument("predecessorMap2Graph: invalid predecessor map!");
+
+	return boost::tuple<G, V, map<string, V> >(g, root, key2vertex);
+}
+
+/* ************************************************************************* */
+/**
+ * Visit each edge and compose the poses
+ */
+template <class V, class Pose, class PoseConfig>
+class compose_key_visitor : public boost::default_bfs_visitor {
+public:
+	compose_key_visitor(PoseConfig& config_in): config(config_in) {}
+	template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
+			string key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
+			string key_to   = boost::get(boost::vertex_name, g, boost::target(edge, g));
+			Pose relativePose = boost::get(boost::edge_weight, g, edge);
+			config.insert(key_to, compose(config.get(key_from), relativePose));
+	}
+
+private:
+	PoseConfig& config;
+
+};
+
+/* ************************************************************************* */
+/**
+ * Compose the poses by following the chain sepcified by the spanning tree
+ */
+template<class G, class Factor, class Pose, class Config>
+Config composePoses(const G& graph, const map<string, string>& tree,
+		const Pose& rootPose) {
+
+	//TODO: change edge_weight_t to edge_pose_t
+	typedef typename boost::adjacency_list<
+		boost::vecS, boost::vecS, boost::directedS,
+		boost::property<boost::vertex_name_t, string>,
+		boost::property<boost::edge_weight_t, Pose> > PoseGraph;
+	typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
+	typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
+
+	PoseGraph g;
+	PoseVertex root;
+	map<string, PoseVertex> key2vertex;
+	boost::tie(g, root, key2vertex) = predecessorMap2Graph<PoseGraph, PoseVertex>(tree);
+
+	// attach the relative poses to the edges
+	PoseEdge edge1, edge2;
+	bool found1, found2;
+	BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) {
+
+		if (nl_factor->keys().size() > 2)
+			throw invalid_argument("composePoses: only support factors with at most two keys");
+		if (nl_factor->keys().size() == 1) continue;
+
+		// e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor
+		boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
+
+		PoseVertex v_from = key2vertex.find(factor->keys().front())->second;
+		PoseVertex v_to = key2vertex.find(factor->keys().back())->second;
+
+		Pose measured = factor->measured();
+		tie(edge1, found1) = boost::edge(v_from, v_to, g);
+		tie(edge2, found2) = boost::edge(v_to, v_from, g);
+		if ((found1 && found2) || (!found1 && !found2))
+				throw invalid_argument ("composePoses: invalid spanning tree");
+		if (found1)
+			boost::put(boost::edge_weight, g, edge1, measured);
+		else if (found2)
+			boost::put(boost::edge_weight, g, edge2, inverse(measured));
+	}
+
+	// compose poses
+	Config config;
+	config.insert(boost::get(boost::vertex_name, g, root), rootPose);
+	compose_key_visitor<PoseVertex, Pose, Config> vis(config);
+	boost::breadth_first_search(g, root, boost::visitor(vis));
+
+	return config;
+}
+
+/* ************************************************************************* */
+
+}

cpp/testGraph.cpp

@@ -0,0 +1,52 @@
+/*
+ * testGraph.cpp
+ *
+ *   Created on: Jan 12, 2010
+ *       Author: nikai
+ *  Description: unit test for graph-inl.h
+ */
+
+#include <iostream>
+#include <boost/shared_ptr.hpp>
+#include <boost/assign/std/list.hpp>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include "Pose2Graph.h"
+#include "graph-inl.h"
+
+using namespace std;
+using namespace boost;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( Graph, composePoses )
+{
+	Pose2Graph graph;
+	Matrix cov = eye(3);
+	graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x1", "x2", Pose2(2.0, 0.0, 0.0), cov)));
+	graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x2", "x3", Pose2(3.0, 0.0, 0.0), cov)));
+
+	map<string, string> tree;
+	tree.insert(make_pair("x1", "x2"));
+	tree.insert(make_pair("x2", "x2"));
+	tree.insert(make_pair("x3", "x2"));
+
+	Pose2 rootPose(3.0, 0.0, 0.0);
+
+	Pose2Config actual = composePoses<Pose2Graph, Pose2Factor, Pose2, Pose2Config>(graph, tree, rootPose);
+	Pose2Config expected;
+	expected.insert("x1", Pose2(1.0, 0.0, 0.0));
+	expected.insert("x2", Pose2(3.0, 0.0, 0.0));
+	expected.insert("x3", Pose2(6.0, 0.0, 0.0));
+
+	LONGS_EQUAL(3, actual.size());
+	CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */