diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h index 46aa075bc..0e06a06f6 100644 --- a/inference/FactorGraph-inl.h +++ b/inference/FactorGraph-inl.h @@ -213,61 +213,61 @@ namespace gtsam { // return ordering; // } -// /* ************************************************************************* */ -// template template -// PredecessorMap FactorGraph::findMinimumSpanningTree() const { -// -// SDGraph g = gtsam::toBoostGraph , Factor2, Key>( -// *this); -// -// // find minimum spanning tree -// vector::Vertex> p_map(boost::num_vertices(g)); -// prim_minimum_spanning_tree(g, &p_map[0]); -// -// // convert edge to string pairs -// PredecessorMap tree; -// typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; -// typename vector::Vertex>::iterator vi; -// for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) { -// Key key = boost::get(boost::vertex_name, g, *itVertex); -// Key parent = boost::get(boost::vertex_name, g, *vi); -// tree.insert(key, parent); -// } -// -// return tree; -// } -// -// /* ************************************************************************* */ -// template template -// void FactorGraph::split(const PredecessorMap& tree, FactorGraph< -// Factor>& Ab1, FactorGraph& Ab2) const { -// -// BOOST_FOREACH(const sharedFactor& factor, factors_) -// { -// if (factor->keys().size() > 2) throw(invalid_argument( -// "split: only support factors with at most two keys")); -// -// if (factor->keys().size() == 1) { -// Ab1.push_back(factor); -// continue; -// } -// -// boost::shared_ptr factor2 = boost::dynamic_pointer_cast< -// Factor2>(factor); -// if (!factor2) continue; -// -// Key key1 = factor2->key1(); -// Key key2 = factor2->key2(); -// // if the tree contains the key -// if ((tree.find(key1) != tree.end() -// && tree.find(key1)->second.compare(key2) == 0) || (tree.find( -// key2) != tree.end() && tree.find(key2)->second.compare(key1) -// == 0)) -// Ab1.push_back(factor2); -// else -// Ab2.push_back(factor2); -// } -// } + /* ************************************************************************* */ + template template + PredecessorMap FactorGraph::findMinimumSpanningTree() const { + + SDGraph g = gtsam::toBoostGraph , Factor2, Key>( + *this); + + // find minimum spanning tree + vector::Vertex> p_map(boost::num_vertices(g)); + prim_minimum_spanning_tree(g, &p_map[0]); + + // convert edge to string pairs + PredecessorMap tree; + typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; + typename vector::Vertex>::iterator vi; + for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) { + Key key = boost::get(boost::vertex_name, g, *itVertex); + Key parent = boost::get(boost::vertex_name, g, *vi); + tree.insert(key, parent); + } + + return tree; + } + + /* ************************************************************************* */ + template template + void FactorGraph::split(const PredecessorMap& tree, + FactorGraph& Ab1, FactorGraph& Ab2) const { + + BOOST_FOREACH(const sharedFactor& factor, factors_) + { + if (factor->keys().size() > 2) throw(invalid_argument( + "split: only support factors with at most two keys")); + + if (factor->keys().size() == 1) { + Ab1.push_back(factor); + continue; + } + + boost::shared_ptr factor2 = boost::dynamic_pointer_cast< + Factor2>(factor); + if (!factor2) continue; + + Key key1 = factor2->key1(); + Key key2 = factor2->key2(); + // if the tree contains the key + if ((tree.find(key1) != tree.end() + && tree.find(key1)->second.compare(key2) == 0) || (tree.find( + key2) != tree.end() && tree.find(key2)->second.compare(key1) + == 0)) + Ab1.push_back(factor2); + else + Ab2.push_back(factor2); + } + } // /* ************************************************************************* */ // template diff --git a/inference/FactorGraph.h b/inference/FactorGraph.h index 7e4923fa7..aa3394733 100644 --- a/inference/FactorGraph.h +++ b/inference/FactorGraph.h @@ -116,18 +116,18 @@ namespace gtsam { // Ordering getOrdering(const std::set& scope) const; // Ordering getConstrainedOrdering(const std::set& lastKeys) const; -// /** -// * find the minimum spanning tree using boost graph library -// */ -// template PredecessorMap -// SL-NEEDED? findMinimumSpanningTree() const; -// -// /** -// * Split the graph into two parts: one corresponds to the given spanning tree, -// * and the other corresponds to the rest of the factors -// */ -// SL-NEEDED? template void split(const PredecessorMap& tree, -// FactorGraph& Ab1, FactorGraph& Ab2) const; + /** + * find the minimum spanning tree using boost graph library + */ + template + PredecessorMap findMinimumSpanningTree() const; + + /** + * Split the graph into two parts: one corresponds to the given spanning tree, + * and the other corresponds to the rest of the factors + */ + template + void split(const PredecessorMap& tree, FactorGraph& Ab1, FactorGraph& Ab2) const; // /** // * find the minimum spanning tree using DSF diff --git a/inference/graph-inl.h b/inference/graph-inl.h index 3d65c0c15..c51b77e8b 100644 --- a/inference/graph-inl.h +++ b/inference/graph-inl.h @@ -48,46 +48,47 @@ list predecessorMap2Keys(const PredecessorMap& p_map) { return keys; } -///* ************************************************************************* */ -//template -// SL-NEEDED? SDGraph toBoostGraph(const G& graph) { -// // convert the factor graph to boost graph -// SDGraph g; -// typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; -// map key2vertex; -// BoostVertex v1, v2; -// typename G::const_iterator itFactor; -// for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) { -// if ((*itFactor)->keys().size() > 2) -// throw(invalid_argument("toBoostGraph: only support factors with at most two keys")); -// -// if ((*itFactor)->keys().size() == 1) -// continue; -// -// boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); -// if (!factor) continue; -// -// Key key1 = factor->key1(); -// Key key2 = factor->key2(); -// -// 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.0); // assume constant edge weight here -// boost::add_edge(v1, v2, edge_property, g); -// } -// -// return g; -//} +/* ************************************************************************* */ +template +SDGraph toBoostGraph(const G& graph) { + // convert the factor graph to boost graph + SDGraph g; + typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; + map key2vertex; + BoostVertex v1, v2; + typename G::const_iterator itFactor; + + for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) { + if ((*itFactor)->keys().size() > 2) + throw(invalid_argument("toBoostGraph: only support factors with at most two keys")); + + if ((*itFactor)->keys().size() == 1) + continue; + + boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); + if (!factor) continue; + + Key key1 = factor->key1(); + Key key2 = factor->key2(); + + 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.0); // assume constant edge weight here + boost::add_edge(v1, v2, edge_property, g); + } + + return g; +} /* ************************************************************************* */ template diff --git a/inference/graph.h b/inference/graph.h index 28d963592..a29ff8bb7 100644 --- a/inference/graph.h +++ b/inference/graph.h @@ -57,13 +57,13 @@ namespace gtsam { template std::list predecessorMap2Keys(const PredecessorMap& p_map); -// /** -// * Convert the factor graph to an SDGraph -// * G = Graph type -// * F = Factor type -// * Key = Key type -// */ -// SL-NEEDED? template SDGraph toBoostGraph(const G& graph); + /** + * Convert the factor graph to an SDGraph + * G = Graph type + * F = Factor type + * Key = Key type + */ + template SDGraph toBoostGraph(const G& graph); /** * Build takes a predecessor map, and builds a directed graph corresponding to the tree.