diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h index 0e06a06f6..ccadadc31 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& 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 aa3394733..b9877277b 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 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 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 c51b77e8b..6bfbc174e 100644 --- a/inference/graph-inl.h +++ b/inference/graph-inl.h @@ -206,4 +206,60 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap +PredecessorMap findMinimumSpanningTree(const G& fg) { + + SDGraph g = gtsam::toBoostGraph(fg); + + // 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 +void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { + + typedef typename G::sharedFactor F ; + + BOOST_FOREACH(const F& factor, g.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); + } +} + } diff --git a/inference/graph.h b/inference/graph.h index a29ff8bb7..fe501672e 100644 --- a/inference/graph.h +++ b/inference/graph.h @@ -80,4 +80,19 @@ namespace gtsam { boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, const Pose& rootPose); + + /** + * find the minimum spanning tree using boost graph library + */ + template + PredecessorMap findMinimumSpanningTree(const G& g) ; + + /** + * 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 G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) ; + + } // namespace gtsam