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

Improved function findMinimumSpanningTree to cope with general graphs (and added comments)

release/4.3a0
Luca 2014-05-14 17:39:59 -04:00
parent 0d957084c0
commit b8300c3b0a
2 changed files with 21 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/inference/graph-inl.h
View File

@ -73,34 +73,41 @@ SDGraph<KEY> toBoostGraph(const G& graph) {
SDGraph<KEY> g;
typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor BoostVertex;
std::map<KEY, BoostVertex> key2vertex;
BoostVertex v1, v2;
typename G::const_iterator itFactor;
// Loop over the factors
for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
if ((*itFactor)->keys().size() > 2)
throw(std::invalid_argument("toBoostGraph: only support factors with at most two keys"));
if ((*itFactor)->keys().size() == 1)
// Ignore factors that are not binary
if ((*itFactor)->keys().size() != 2)
continue;
// Cast the factor to the user-specified factor type F
boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
// Ignore factors that are not of type F
if (!factor) continue;
KEY key1 = factor->key1();
KEY key2 = factor->key2();
// Retrieve the 2 keys (nodes) the factor (edge) is incident on
KEY key1 = factor->keys()[0];
KEY key2 = factor->keys()[1];
BoostVertex v1, v2;
// If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id
if (key2vertex.find(key1) == key2vertex.end()) {
v1 = add_vertex(key1, g);
key2vertex.insert(std::pair<KEY,KEY>(key1, v1));
} else
v1 = key2vertex[key1];
// If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id
if (key2vertex.find(key2) == key2vertex.end()) {
v2 = add_vertex(key2, g);
key2vertex.insert(std::pair<KEY,KEY>(key2, v2));
} else
v2 = key2vertex[key2];
// Add an edge with weight 1.0
boost::property<boost::edge_weight_t, double> edge_property(1.0); // assume constant edge weight here
boost::add_edge(v1, v2, edge_property, g);
}
@ -222,12 +229,11 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
return config;
}
/* ************************************************************************* */
/* ************************************************************************* */
template<class G, class KEY, class FACTOR2>
PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
// Convert to a graph that boost understands
SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
// find minimum spanning tree

27
tests/testGraph.cpp
View File

@ -109,19 +109,6 @@ TEST( Graph, composePoses )
}
///* ************************************************************************* */
// A linear factor implementing the functions key1 and key2
// needed for findMinimumSpanningTree
class Factor2 : public JacobianFactor {
public:
/** Construct binary factor */
Factor2(Key i1, const Matrix& A1, Key i2, const Matrix& A2, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianFactor(i1, A1, i2, A2, b, model) {
}
Key key1() const {return keys_[0];}
Key key2() const {return keys_[1];}
};
TEST( GaussianFactorGraph, findMinimumSpanningTree )
{
@ -130,14 +117,14 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree )
Vector2 b(0, 0);
const SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
using namespace symbol_shorthand;
g += Factor2(X(1), I, X(2), I, b, model);
g += Factor2(X(1), I, X(3), I, b, model);
g += Factor2(X(1), I, X(4), I, b, model);
g += Factor2(X(2), I, X(3), I, b, model);
g += Factor2(X(2), I, X(4), I, b, model);
g += Factor2(X(3), I, X(4), I, b, model);
g += JacobianFactor(X(1), I, X(2), I, b, model);
g += JacobianFactor(X(1), I, X(3), I, b, model);
g += JacobianFactor(X(1), I, X(4), I, b, model);
g += JacobianFactor(X(2), I, X(3), I, b, model);
g += JacobianFactor(X(2), I, X(4), I, b, model);
g += JacobianFactor(X(3), I, X(4), I, b, model);
PredecessorMap<Key> tree = findMinimumSpanningTree<GaussianFactorGraph, Key, Factor2>(g);
PredecessorMap<Key> tree = findMinimumSpanningTree<GaussianFactorGraph, Key, JacobianFactor>(g);
EXPECT_LONGS_EQUAL(tree[X(1)], X(1));
EXPECT_LONGS_EQUAL(tree[X(2)], X(1));
EXPECT_LONGS_EQUAL(tree[X(3)], X(1));