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

fix for cityslam

release/4.3a0
Yong-Dian Jian 2010-10-13 22:58:58 +00:00
parent 2ce0f40b79
commit 129fc2c997
4 changed files with 115 additions and 114 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, FactorGraph< void FactorGraph<Factor>::split(const PredecessorMap<Key>& tree,
// 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> PredecessorMap<Key> template<class Key, class Factor2>
// SL-NEEDED? 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
// */ */
// SL-NEEDED? template<class Key, class Factor2> void split(const PredecessorMap<Key>& tree, template<class Key, class Factor2>
// 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

81
inference/graph-inl.h
View File

@ -48,46 +48,47 @@ list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map) {
return keys; return keys;
} }
///* ************************************************************************* */ /* ************************************************************************* */
//template<class G, class F, class Key> template<class G, class F, class Key>
// SL-NEEDED? SDGraph<Key> toBoostGraph(const G& graph) { SDGraph<Key> toBoostGraph(const G& graph) {
// // convert the factor graph to boost graph // convert the factor graph to boost graph
// SDGraph<Key> g; SDGraph<Key> g;
// typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex; typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex;
// map<Key, BoostVertex> key2vertex; map<Key, BoostVertex> key2vertex;
// BoostVertex v1, v2; BoostVertex v1, v2;
// typename G::const_iterator itFactor; typename G::const_iterator itFactor;
// for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
// if ((*itFactor)->keys().size() > 2) for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
// throw(invalid_argument("toBoostGraph: only support factors with at most two keys")); if ((*itFactor)->keys().size() > 2)
// throw(invalid_argument("toBoostGraph: only support factors with at most two keys"));
// if ((*itFactor)->keys().size() == 1)
// continue; if ((*itFactor)->keys().size() == 1)
// continue;
// boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
// if (!factor) continue; boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
// if (!factor) continue;
// Key key1 = factor->key1();
// Key key2 = factor->key2(); Key key1 = factor->key1();
// Key key2 = factor->key2();
// if (key2vertex.find(key1) == key2vertex.end()) {
// v1 = add_vertex(key1, g); if (key2vertex.find(key1) == key2vertex.end()) {
// key2vertex.insert(make_pair(key1, v1)); v1 = add_vertex(key1, g);
// } else key2vertex.insert(make_pair(key1, v1));
// v1 = key2vertex[key1]; } else
// v1 = key2vertex[key1];
// if (key2vertex.find(key2) == key2vertex.end()) {
// v2 = add_vertex(key2, g); if (key2vertex.find(key2) == key2vertex.end()) {
// key2vertex.insert(make_pair(key2, v2)); v2 = add_vertex(key2, g);
// } else key2vertex.insert(make_pair(key2, v2));
// v2 = key2vertex[key2]; } 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); 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;
//} return g;
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class V, class Key> template<class G, class V, class Key>

14
inference/graph.h
View File

@ -57,13 +57,13 @@ namespace gtsam {
template<class Key> template<class Key>
std::list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map); std::list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map);
// /** /**
// * Convert the factor graph to an SDGraph * Convert the factor graph to an SDGraph
// * G = Graph type * G = Graph type
// * F = Factor type * F = Factor type
// * Key = Key type * Key = Key type
// */ */
// SL-NEEDED? template<class G, class F, class Key> SDGraph<Key> toBoostGraph(const G& graph); template<class G, class F, class Key> SDGraph<Key> toBoostGraph(const G& graph);
/** /**
* Build takes a predecessor map, and builds a directed graph corresponding to the tree. * Build takes a predecessor map, and builds a directed graph corresponding to the tree.