12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

move min spanning tree and split functions from FactorGraph.h to graph.h

release/4.3a0
Yong-Dian Jian 2010-10-14 02:07:37 +00:00
parent 422a4ae16d
commit fd794e0da2
4 changed files with 138 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
inference/FactorGraph-inl.h
View File

@ -213,61 +213,61 @@ namespace gtsam {
// return ordering; // return ordering;
// } // }
/* ************************************************************************* */ // /* ************************************************************************* */
template<class Factor> template<class Key, class Factor2> // template<class Factor> template<class Key, class Factor2>
PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const { // PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
//
SDGraph<Key> g = gtsam::toBoostGraph<FactorGraph<Factor> , Factor2, Key>( // SDGraph<Key> g = gtsam::toBoostGraph<FactorGraph<Factor> , Factor2, Key>(
*this); // *this);
//
// find minimum spanning tree // // find minimum spanning tree
vector<typename SDGraph<Key>::Vertex> p_map(boost::num_vertices(g)); // vector<typename SDGraph<Key>::Vertex> p_map(boost::num_vertices(g));
prim_minimum_spanning_tree(g, &p_map[0]); // prim_minimum_spanning_tree(g, &p_map[0]);
//
// convert edge to string pairs // // convert edge to string pairs
PredecessorMap<Key> tree; // PredecessorMap<Key> tree;
typename SDGraph<Key>::vertex_iterator itVertex = boost::vertices(g).first; // typename SDGraph<Key>::vertex_iterator itVertex = boost::vertices(g).first;
typename vector<typename SDGraph<Key>::Vertex>::iterator vi; // typename vector<typename SDGraph<Key>::Vertex>::iterator vi;
for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) { // for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) {
Key key = boost::get(boost::vertex_name, g, *itVertex); // Key key = boost::get(boost::vertex_name, g, *itVertex);
Key parent = boost::get(boost::vertex_name, g, *vi); // Key parent = boost::get(boost::vertex_name, g, *vi);
tree.insert(key, parent); // tree.insert(key, parent);
} // }
//
return tree; // return tree;
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
template<class Factor> template<class Key, class Factor2> // template<class Factor> template<class Key, class Factor2>
void FactorGraph<Factor>::split(const PredecessorMap<Key>& tree, // void FactorGraph<Factor>::split(const PredecessorMap<Key>& tree,
FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const { // FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const {
//
BOOST_FOREACH(const sharedFactor& factor, factors_) // BOOST_FOREACH(const sharedFactor& factor, factors_)
{ // {
if (factor->keys().size() > 2) throw(invalid_argument( // if (factor->keys().size() > 2) throw(invalid_argument(
"split: only support factors with at most two keys")); // "split: only support factors with at most two keys"));
//
if (factor->keys().size() == 1) { // if (factor->keys().size() == 1) {
Ab1.push_back(factor); // Ab1.push_back(factor);
continue; // continue;
} // }
//
boost::shared_ptr<Factor2> factor2 = boost::dynamic_pointer_cast< // boost::shared_ptr<Factor2> factor2 = boost::dynamic_pointer_cast<
Factor2>(factor); // Factor2>(factor);
if (!factor2) continue; // if (!factor2) continue;
//
Key key1 = factor2->key1(); // Key key1 = factor2->key1();
Key key2 = factor2->key2(); // Key key2 = factor2->key2();
// if the tree contains the key // // if the tree contains the key
if ((tree.find(key1) != tree.end() // if ((tree.find(key1) != tree.end()
&& tree.find(key1)->second.compare(key2) == 0) || (tree.find( // && tree.find(key1)->second.compare(key2) == 0) || (tree.find(
key2) != tree.end() && tree.find(key2)->second.compare(key1) // key2) != tree.end() && tree.find(key2)->second.compare(key1)
== 0)) // == 0))
Ab1.push_back(factor2); // Ab1.push_back(factor2);
else // else
Ab2.push_back(factor2); // Ab2.push_back(factor2);
} // }
} // }
// /* ************************************************************************* */ // /* ************************************************************************* */
// template<class Factor> // template<class Factor>

24
inference/FactorGraph.h
View File

@ -116,18 +116,18 @@ namespace gtsam {
// Ordering getOrdering(const std::set<Index>& scope) const; // Ordering getOrdering(const std::set<Index>& scope) const;
// Ordering getConstrainedOrdering(const std::set<Index>& lastKeys) const; // Ordering getConstrainedOrdering(const std::set<Index>& lastKeys) const;
/** // /**
* find the minimum spanning tree using boost graph library // * find the minimum spanning tree using boost graph library
*/ // */
template<class Key, class Factor2> // template<class Key, class Factor2>
PredecessorMap<Key> findMinimumSpanningTree() const; // PredecessorMap<Key> findMinimumSpanningTree() const;
//
/** // /**
* Split the graph into two parts: one corresponds to the given spanning tree, // * Split the graph into two parts: one corresponds to the given spanning tree,
* and the other corresponds to the rest of the factors // * and the other corresponds to the rest of the factors
*/ // */
template<class Key, class Factor2> // template<class Key, class Factor2>
void split(const PredecessorMap<Key>& tree, FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const; // void split(const PredecessorMap<Key>& tree, FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const;
// /** // /**
// * find the minimum spanning tree using DSF // * find the minimum spanning tree using DSF

56
inference/graph-inl.h
View File

@ -206,4 +206,60 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<type
/* ************************************************************************* */ /* ************************************************************************* */
/* ************************************************************************* */
template<class G, class Key, class Factor2>
PredecessorMap<Key> findMinimumSpanningTree(const G& fg) {
SDGraph<Key> g = gtsam::toBoostGraph<G, Factor2, Key>(fg);
// find minimum spanning tree
vector<typename SDGraph<Key>::Vertex> p_map(boost::num_vertices(g));
prim_minimum_spanning_tree(g, &p_map[0]);
// convert edge to string pairs
PredecessorMap<Key> tree;
typename SDGraph<Key>::vertex_iterator itVertex = boost::vertices(g).first;
typename vector<typename SDGraph<Key>::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<class G, class Key, class Factor2>
void split(const G& g, const PredecessorMap<Key>& 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> 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);
}
}
} }

15
inference/graph.h
View File

@ -80,4 +80,19 @@ namespace gtsam {
boost::shared_ptr<Values> boost::shared_ptr<Values>
composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree, const Pose& rootPose); composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree, const Pose& rootPose);
/**
* find the minimum spanning tree using boost graph library
*/
template<class G, class Key, class Factor2>
PredecessorMap<Key> 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<class G, class Key, class Factor2>
void split(const G& g, const PredecessorMap<Key>& tree, G& Ab1, G& Ab2) ;
} // namespace gtsam } // namespace gtsam