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

1) add a compose function that compose all the poses in a factor graph given the spanning tree 

2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
release/4.3a0
Kai Ni 2010-01-12 16:12:25 +00:00
parent bea55b5f5b
commit 4369cd2d92
9 changed files with 798 additions and 589 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1021
.cproject
View File

File diff suppressed because it is too large Load Diff

3
cpp/BetweenFactor.h
View File

@ -79,6 +79,9 @@ namespace gtsam {
inline const std::string& key1() const { return key1_;}
inline const std::string& key2() const { return key2_;}
/** return the measured */
inline const T measured() const {return measured_;}
/** keys as a list */
inline std::list<std::string> keys() const { return keys_;}

46
cpp/FactorGraph-inl.h
View File

@ -18,8 +18,6 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/format.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <colamd/colamd.h>
#include "Ordering.h"
@ -275,52 +273,16 @@ void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
template<class Factor>
map<string, string> FactorGraph<Factor>::findMinimumSpanningTree() const {
typedef boost::adjacency_list<
boost::vecS, boost::vecS, boost::undirectedS,
boost::property<boost::vertex_name_t, string>,
boost::property<boost::edge_weight_t, int> > Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
typedef boost::graph_traits<Graph>::vertex_iterator VertexIterator;
typedef boost::graph_traits<Graph>::edge_descriptor Edge;
// convert the factor graph to boost graph
Graph g(0);
map<string, Vertex> key2vertex;
Vertex v1, v2;
BOOST_FOREACH(sharedFactor factor, factors_){
if (factor->keys().size() > 2)
throw(invalid_argument("findMinimumSpanningTree: only support factors with at most two keys"));
if (factor->keys().size() == 1)
continue;
string key1 = factor->keys().front();
string key2 = factor->keys().back();
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<boost::edge_weight_t, int> edge_property(1); // assume constant edge weight here
boost::add_edge(v1, v2, edge_property, g);
}
SDGraph g = gtsam::toBoostGraph<FactorGraph<Factor>, sharedFactor>(*this);
// find minimum spanning tree
vector<Vertex> p_map(boost::num_vertices(g));
vector<BoostVertex> p_map(boost::num_vertices(g));
prim_minimum_spanning_tree(g, &p_map[0]);
// convert edge to string pairs
map<string, string> tree;
VertexIterator itVertex = boost::vertices(g).first;
for (vector<Vertex>::iterator vi = p_map.begin(); vi!=p_map.end(); itVertex++, vi++) {
BoostVertexIterator itVertex = boost::vertices(g).first;
for (vector<BoostVertex>::iterator 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);
// printf("%s parent: %s\n", key.c_str(), parent.c_str());

1
cpp/FactorGraph.h
View File

@ -17,6 +17,7 @@
#include "Testable.h"
#include "BayesNet.h"
#include "graph-inl.h"
namespace gtsam {

5
cpp/Makefile.am
View File

@ -66,14 +66,17 @@ testSymbolicBayesNet_LDADD = libgtsam.la
# Inference
headers += inference.h inference-inl.h
headers += graph-inl.h
headers += FactorGraph.h FactorGraph-inl.h
headers += BayesNet.h BayesNet-inl.h
headers += BayesTree.h BayesTree-inl.h
headers += ISAM.h ISAM-inl.h GaussianISAM.h
headers += ISAM2.h ISAM2-inl.h GaussianISAM2.h
sources += GaussianISAM.cpp GaussianISAM2.cpp
check_PROGRAMS += testFactorgraph testInference testOrdering
check_PROGRAMS += testGraph testFactorgraph testInference testOrdering
check_PROGRAMS += testBayesTree testISAM testGaussianISAM testGaussianISAM2
testGraph_SOURCES = testGraph.cpp
testGraph_LDADD = libgtsam.la
testFactorgraph_SOURCES = testFactorgraph.cpp
testInference_SOURCES = $(example) testInference.cpp
testFactorgraph_LDADD = libgtsam.la

57
cpp/Ordering.cpp
View File

@ -9,65 +9,44 @@
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include "graph-inl.h"
#include "Ordering.h"
using namespace std;
using namespace gtsam;
using namespace boost::assign;
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
class key_visitor : public boost::default_bfs_visitor {
class ordering_key_visitor : public boost::default_bfs_visitor {
public:
key_visitor(Ordering& ordering_in) : ordering_(ordering_in) {}
template <typename Vertex, typename Graph> void discover_vertex(Vertex u, const Graph& g) const {
string key = boost::get(boost::vertex_name, g, u);
ordering_key_visitor(Ordering& ordering_in) : ordering_(ordering_in) {}
template <typename Vertex, typename Graph> void discover_vertex(Vertex v, const Graph& g) const {
string key = boost::get(boost::vertex_name, g, v);
ordering_.push_front(key);
}
Ordering& ordering_;
};
class ordering_edge_action {
public:
void act (string& child, string& parent, SVertex& v1, SVertex& v2, SGraph& g){
//boost::add_edge(v1, v2, g);
}
};
/* ************************************************************************* */
Ordering::Ordering(const map<string, string>& p_map) {
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
boost::property<boost::vertex_name_t, string> > Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
// build the graph corresponding to the predecessor map
Graph g(0);
map<string, Vertex> key2vertex;
Vertex v1, v2, root;
string child, parent;
bool foundRoot = false;
FOREACH_PAIR(child, parent, p_map) {
if (key2vertex.find(child) == key2vertex.end()) {
v1 = add_vertex(child, g);
key2vertex.insert(make_pair(child, v1));
} else
v1 = key2vertex[child];
if (key2vertex.find(parent) == key2vertex.end()) {
v2 = add_vertex(parent, g);
key2vertex.insert(make_pair(parent, v2));
} else
v2 = key2vertex[parent];
if (child.compare(parent) == 0) {
root = v1;
foundRoot = true;
} else
boost::add_edge(v1, v2, g);
}
if (!foundRoot)
throw invalid_argument("Ordering: invalid predecessor map!");
SGraph g;
SVertex root;
map<string, SVertex> key2vertex;
boost::tie(g, root, key2vertex) = predecessorMap2Graph<SGraph, SVertex>(p_map);
// breadth first visit on the graph
key_visitor vis(*this);
ordering_key_visitor vis(*this);
boost::breadth_first_search(g, root, boost::visitor(vis));
}

9
cpp/Pose2Graph.cpp
View File

@ -3,11 +3,14 @@
* @brief A factor graph for the 2D PoseSLAM problem
* @authors Frank Dellaert, Viorela Ila
*/
//#include "NonlinearOptimizer-inl.h"
#include "FactorGraph-inl.h"
#include "NonlinearFactorGraph-inl.h"
#include "graph-inl.h"
#include "Pose2Graph.h"
using namespace std;
using namespace gtsam;
namespace gtsam {
// explicit instantiation so all the code is there and we can link with it
@ -15,8 +18,12 @@ template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ;
template class NonlinearFactorGraph<Pose2Config> ;
//template class NonlinearOptimizer<Pose2Graph, Pose2Config> ;
/* ************************************************************************* */
bool Pose2Graph::equals(const Pose2Graph& p, double tol) const {
return false;
}
/* ************************************************************************* */
} // namespace gtsam

193
cpp/graph-inl.h Normal file
View File

@ -0,0 +1,193 @@
/*
* graph-inl.h
*
* Created on: Jan 11, 2010
* Author: nikai
* Description: Graph algorithm using boost library
*/
#pragma once
#include <boost/foreach.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
using namespace std;
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
/* ************************************************************************* */
/**
* type definitions
*/
typedef boost::adjacency_list<
boost::vecS, boost::vecS, boost::undirectedS,
boost::property<boost::vertex_name_t, std::string>,
boost::property<boost::edge_weight_t, double> > SDGraph;
typedef boost::graph_traits<SDGraph>::vertex_descriptor BoostVertex;
typedef boost::graph_traits<SDGraph>::vertex_iterator BoostVertexIterator;
typedef boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, string> > SGraph;
typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
/* ************************************************************************* */
/**
* Convert the factor graph to a boost undirected graph
*/
template<class G, class F>
SDGraph toBoostGraph(const G& graph) {
// convert the factor graph to boost graph
SDGraph g(0);
map<string, BoostVertex> key2vertex;
BoostVertex v1, v2;
BOOST_FOREACH(F factor, graph) {
if (factor->keys().size() > 2)
throw(invalid_argument("toBoostGraph: only support factors with at most two keys"));
if (factor->keys().size() == 1)
continue;
string key1 = factor->keys().front();
string key2 = factor->keys().back();
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<boost::edge_weight_t, double> edge_property(1.0); // assume constant edge weight here
boost::add_edge(v1, v2, edge_property, g);
}
return g;
}
/* ************************************************************************* */
/**
* build the graph corresponding to the predecessor map. Excute action for each edge.
*/
template<class G, class V>
boost::tuple<G, V, map<string, V> > predecessorMap2Graph(const map<string, string>& p_map) {
G g(0);
map<string, V> key2vertex;
V v1, v2, root;
string child, parent;
bool foundRoot = false;
FOREACH_PAIR(child, parent, p_map) {
if (key2vertex.find(child) == key2vertex.end()) {
v1 = add_vertex(child, g);
key2vertex.insert(make_pair(child, v1));
} else
v1 = key2vertex[child];
if (key2vertex.find(parent) == key2vertex.end()) {
v2 = add_vertex(parent, g);
key2vertex.insert(make_pair(parent, v2));
} else
v2 = key2vertex[parent];
if (child.compare(parent) == 0) {
root = v1;
foundRoot = true;
} else
boost::add_edge(v2, v1, g); // edge is from parent to child
}
if (!foundRoot)
throw invalid_argument("predecessorMap2Graph: invalid predecessor map!");
return boost::tuple<G, V, map<string, V> >(g, root, key2vertex);
}
/* ************************************************************************* */
/**
* Visit each edge and compose the poses
*/
template <class V, class Pose, class PoseConfig>
class compose_key_visitor : public boost::default_bfs_visitor {
public:
compose_key_visitor(PoseConfig& config_in): config(config_in) {}
template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
string key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
string key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
Pose relativePose = boost::get(boost::edge_weight, g, edge);
config.insert(key_to, compose(config.get(key_from), relativePose));
}
private:
PoseConfig& config;
};
/* ************************************************************************* */
/**
* Compose the poses by following the chain sepcified by the spanning tree
*/
template<class G, class Factor, class Pose, class Config>
Config composePoses(const G& graph, const map<string, string>& tree,
const Pose& rootPose) {
//TODO: change edge_weight_t to edge_pose_t
typedef typename boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, string>,
boost::property<boost::edge_weight_t, Pose> > PoseGraph;
typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
PoseGraph g;
PoseVertex root;
map<string, PoseVertex> key2vertex;
boost::tie(g, root, key2vertex) = predecessorMap2Graph<PoseGraph, PoseVertex>(tree);
// attach the relative poses to the edges
PoseEdge edge1, edge2;
bool found1, found2;
BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) {
if (nl_factor->keys().size() > 2)
throw invalid_argument("composePoses: only support factors with at most two keys");
if (nl_factor->keys().size() == 1) continue;
// e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
PoseVertex v_from = key2vertex.find(factor->keys().front())->second;
PoseVertex v_to = key2vertex.find(factor->keys().back())->second;
Pose measured = factor->measured();
tie(edge1, found1) = boost::edge(v_from, v_to, g);
tie(edge2, found2) = boost::edge(v_to, v_from, g);
if ((found1 && found2) || (!found1 && !found2))
throw invalid_argument ("composePoses: invalid spanning tree");
if (found1)
boost::put(boost::edge_weight, g, edge1, measured);
else if (found2)
boost::put(boost::edge_weight, g, edge2, inverse(measured));
}
// compose poses
Config config;
config.insert(boost::get(boost::vertex_name, g, root), rootPose);
compose_key_visitor<PoseVertex, Pose, Config> vis(config);
boost::breadth_first_search(g, root, boost::visitor(vis));
return config;
}
/* ************************************************************************* */
}

52
cpp/testGraph.cpp Normal file
View File

@ -0,0 +1,52 @@
/*
* testGraph.cpp
*
* Created on: Jan 12, 2010
* Author: nikai
* Description: unit test for graph-inl.h
*/
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp>
#include <CppUnitLite/TestHarness.h>
#include "Pose2Graph.h"
#include "graph-inl.h"
using namespace std;
using namespace boost;
using namespace gtsam;
/* ************************************************************************* */
TEST( Graph, composePoses )
{
Pose2Graph graph;
Matrix cov = eye(3);
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x1", "x2", Pose2(2.0, 0.0, 0.0), cov)));
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x2", "x3", Pose2(3.0, 0.0, 0.0), cov)));
map<string, string> tree;
tree.insert(make_pair("x1", "x2"));
tree.insert(make_pair("x2", "x2"));
tree.insert(make_pair("x3", "x2"));
Pose2 rootPose(3.0, 0.0, 0.0);
Pose2Config actual = composePoses<Pose2Graph, Pose2Factor, Pose2, Pose2Config>(graph, tree, rootPose);
Pose2Config expected;
expected.insert("x1", Pose2(1.0, 0.0, 0.0));
expected.insert("x2", Pose2(3.0, 0.0, 0.0));
expected.insert("x3", Pose2(6.0, 0.0, 0.0));
LONGS_EQUAL(3, actual.size());
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */