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

ordering is fixed

release/4.3a0
Kai Ni 2010-01-14 03:21:07 +00:00
parent ac10c440e1
commit 68d90772e6
6 changed files with 36 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/FactorGraph-inl.h
View File

@ -274,19 +274,16 @@ void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
template<class Factor> template <class Key>
map<Key,Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex;
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_iterator BoostVertexIterator;
SDGraph<Key> g = gtsam::toBoostGraph<FactorGraph<Factor>, sharedFactor, Key>(*this);
// find minimum spanning tree
vector<BoostVertex> 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]);
// convert edge to string pairs
map<Key, Key> tree;
BoostVertexIterator itVertex = boost::vertices(g).first;
typename vector<BoostVertex>::iterator vi;
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++) {
string key = boost::get(boost::vertex_name, g, *itVertex);
string parent = boost::get(boost::vertex_name, g, *vi);

2
cpp/Ordering.cpp
View File

@ -38,7 +38,7 @@ Ordering::Ordering(const PredecessorMap<string>& p_map) {
SGraph<string> g;
SVertex root;
map<string, SVertex> key2vertex;
//boost::tie(g, root, key2vertex) = predecessorMap2Graph<string>(p_map);
boost::tie(g, root, key2vertex) = predecessorMap2Graph<SGraph<string>, SVertex, string>(p_map);
// breadth first visit on the graph
ordering_key_visitor vis(*this);

13
cpp/graph-inl.h
View File

@ -61,12 +61,11 @@ SDGraph<Key> toBoostGraph(const G& graph) {
}
/* ************************************************************************* */
template<class Key>
boost::tuple<SDGraph<Key>, typename SDGraph<Key>::Vertex, std::map<Key, typename SDGraph<Key>::Vertex> >
template<class G, class V, class Key>
boost::tuple<G, V, map<Key,V> >
predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
typedef typename SDGraph<Key>::Vertex V;
SDGraph<Key> g;
G g;
map<Key, V> key2vertex;
V v1, v2, root;
Key child, parent;
@ -94,7 +93,7 @@ predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
if (!foundRoot)
throw invalid_argument("predecessorMap2Graph: invalid predecessor map!");
return boost::tuple<SDGraph<Key>, V, std::map<Key, V> >(g, root, key2vertex);
return boost::tuple<G, V, std::map<Key, V> >(g, root, key2vertex);
}
/* ************************************************************************* */
@ -133,8 +132,8 @@ boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<type
PoseGraph g;
PoseVertex root;
map<typename Config::Key, PoseVertex> key2vertex;
// boost::tie(g, root, key2vertex) =
// predecessorMap2Graph<typename Config::Key>(tree);
boost::tie(g, root, key2vertex) =
predecessorMap2Graph<PoseGraph, PoseVertex, typename Config::Key>(tree);
// attach the relative poses to the edges
PoseEdge edge1, edge2;

11
cpp/graph.h
View File

@ -24,13 +24,15 @@ namespace gtsam {
class SDGraph: public boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
boost::property<boost::vertex_name_t, Key>, boost::property<
boost::edge_weight_t, double> > {
public:
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor Vertex;
};
template<class Key>
class SGraph : public boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, std::string> > {
boost::property<boost::vertex_name_t, Key> > {
public:
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor Vertex;
typedef typename boost::graph_traits<SGraph<Key> >::vertex_descriptor Vertex;
};
//typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
@ -54,9 +56,8 @@ namespace gtsam {
* G = Graph type
* V = Vertex type
*/
template<class Key>
boost::tuple<SDGraph<Key>, typename SDGraph<Key>::Vertex, std::map<Key, typename SDGraph<Key>::Vertex> >
predecessorMap2Graph(const PredecessorMap<Key>& p_map);
template<class G, class V, class Key>
boost::tuple<G, V, std::map<Key,V> > predecessorMap2Graph(const PredecessorMap<Key>& p_map);
/**
* Compose the poses by following the chain specified by the spanning tree

18
cpp/testGraph.cpp
View File

@ -20,6 +20,24 @@ using namespace std;
using namespace boost;
using namespace gtsam;
/* ************************************************************************* */
TEST( Graph, predecessorMap2Graph )
{
typedef SGraph<string>::Vertex SVertex;
SGraph<string> graph;
SVertex root;
map<string, SVertex> key2vertex;
PredecessorMap<string> p_map;
p_map.insert(make_pair("x1", "x2"));
p_map.insert(make_pair("x2", "x2"));
p_map.insert(make_pair("x3", "x2"));
tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<string>, SVertex, string>(p_map);
LONGS_EQUAL(3, boost::num_vertices(graph));
CHECK(root == key2vertex["x2"]);
}
/* ************************************************************************* */
TEST( Graph, composePoses )
{

4
cpp/testOrdering.cpp
View File

@ -11,12 +11,12 @@ using namespace std;
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* *
/* ************************************************************************* */
// x1 -> x2
// -> x3 -> x4
// -> x5
TEST ( Ordering, constructorFromTree ) {
map<string, string> p_map;
PredecessorMap<string> p_map;
p_map.insert(make_pair("x1", "x1"));
p_map.insert(make_pair("x2", "x1"));
p_map.insert(make_pair("x3", "x1"));