2010-01-13 00:12:25 +08:00
|
|
|
/*
|
|
|
|
|
* graph-inl.h
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
* @brief Graph algorithm using boost library
|
|
|
|
|
* @author: Kai Ni
|
2010-01-13 00:12:25 +08:00
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
#pragma once
|
|
|
|
|
|
|
|
|
|
#include <boost/foreach.hpp>
|
|
|
|
|
#include <boost/graph/breadth_first_search.hpp>
|
2010-01-14 03:36:52 +08:00
|
|
|
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
#include "graph.h"
|
2010-01-13 00:12:25 +08:00
|
|
|
|
|
|
|
|
using namespace std;
|
|
|
|
|
|
|
|
|
|
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
|
|
|
|
|
|
|
|
|
namespace gtsam {
|
|
|
|
|
|
2010-01-19 03:11:22 +08:00
|
|
|
/* ************************************************************************* */
|
|
|
|
|
template <class Key>
|
|
|
|
|
class ordering_key_visitor : public boost::default_bfs_visitor {
|
|
|
|
|
public:
|
|
|
|
|
ordering_key_visitor(std::list<Key>& ordering_in) : ordering_(ordering_in) {}
|
|
|
|
|
template <typename Vertex, typename Graph> void discover_vertex(Vertex v, const Graph& g) const {
|
|
|
|
|
Key key = boost::get(boost::vertex_name, g, v);
|
|
|
|
|
ordering_.push_front(key);
|
|
|
|
|
}
|
|
|
|
|
std::list<Key>& ordering_;
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
/* ************************************************************************* */
|
|
|
|
|
template<class Key>
|
|
|
|
|
list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map) {
|
|
|
|
|
|
|
|
|
|
typedef typename SGraph<Key>::Vertex SVertex;
|
|
|
|
|
|
|
|
|
|
SGraph<Key> g;
|
|
|
|
|
SVertex root;
|
|
|
|
|
std::map<Key, SVertex> key2vertex;
|
|
|
|
|
boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
|
|
|
|
|
|
|
|
|
|
// breadth first visit on the graph
|
|
|
|
|
std::list<Key> keys;
|
|
|
|
|
ordering_key_visitor<Key> vis(keys);
|
|
|
|
|
boost::breadth_first_search(g, root, boost::visitor(vis));
|
|
|
|
|
return keys;
|
|
|
|
|
}
|
|
|
|
|
|
2010-01-13 00:12:25 +08:00
|
|
|
/* ************************************************************************* */
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
template<class G, class F, class Key>
|
|
|
|
|
SDGraph<Key> toBoostGraph(const G& graph) {
|
2010-01-13 00:12:25 +08:00
|
|
|
// convert the factor graph to boost graph
|
2010-01-14 07:59:46 +08:00
|
|
|
SDGraph<Key> g;
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex;
|
|
|
|
|
map<Key, BoostVertex> key2vertex;
|
2010-01-13 00:12:25 +08:00
|
|
|
BoostVertex v1, v2;
|
2010-01-16 09:16:59 +08:00
|
|
|
typename G::const_iterator itFactor;
|
|
|
|
|
for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
|
|
|
|
|
if ((*itFactor)->keys().size() > 2)
|
2010-01-13 00:12:25 +08:00
|
|
|
throw(invalid_argument("toBoostGraph: only support factors with at most two keys"));
|
|
|
|
|
|
2010-01-16 09:16:59 +08:00
|
|
|
if ((*itFactor)->keys().size() == 1)
|
2010-01-13 00:12:25 +08:00
|
|
|
continue;
|
|
|
|
|
|
2010-01-16 09:16:59 +08:00
|
|
|
boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
|
|
|
|
|
if (!factor) continue;
|
|
|
|
|
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
Key key1 = factor->key1();
|
|
|
|
|
Key key2 = factor->key2();
|
2010-01-13 00:12:25 +08:00
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* ************************************************************************* */
|
2010-01-14 11:21:07 +08:00
|
|
|
template<class G, class V, class Key>
|
|
|
|
|
boost::tuple<G, V, map<Key,V> >
|
2010-01-14 09:25:40 +08:00
|
|
|
predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
|
2010-01-13 00:12:25 +08:00
|
|
|
|
2010-01-14 11:21:07 +08:00
|
|
|
G g;
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
map<Key, V> key2vertex;
|
2010-01-13 00:12:25 +08:00
|
|
|
V v1, v2, root;
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
Key child, parent;
|
2010-01-13 00:12:25 +08:00
|
|
|
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];
|
|
|
|
|
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
if (child==parent) {
|
2010-01-13 00:12:25 +08:00
|
|
|
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!");
|
|
|
|
|
|
2010-01-14 11:21:07 +08:00
|
|
|
return boost::tuple<G, V, std::map<Key, V> >(g, root, key2vertex);
|
2010-01-13 00:12:25 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* ************************************************************************* */
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
template <class V,class Pose, class Config>
|
2010-01-13 00:12:25 +08:00
|
|
|
class compose_key_visitor : public boost::default_bfs_visitor {
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
|
|
|
|
|
private:
|
|
|
|
|
boost::shared_ptr<Config> config_;
|
|
|
|
|
|
2010-01-13 00:12:25 +08:00
|
|
|
public:
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
|
|
|
|
|
compose_key_visitor(boost::shared_ptr<Config> config_in) {config_ = config_in;}
|
|
|
|
|
|
2010-01-13 00:12:25 +08:00
|
|
|
template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
typename Config::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
|
|
|
|
|
typename Config::Key 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(relativePose, (*config_)[key_from]));
|
2010-01-13 00:12:25 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
/* ************************************************************************* */
|
|
|
|
|
template<class G, class Factor, class Pose, class Config>
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<typename Config::Key>& tree,
|
2010-01-13 00:12:25 +08:00
|
|
|
const Pose& rootPose) {
|
|
|
|
|
|
|
|
|
|
//TODO: change edge_weight_t to edge_pose_t
|
|
|
|
|
typedef typename boost::adjacency_list<
|
|
|
|
|
boost::vecS, boost::vecS, boost::directedS,
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
boost::property<boost::vertex_name_t, typename Config::Key>,
|
2010-01-13 00:12:25 +08:00
|
|
|
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;
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
map<typename Config::Key, PoseVertex> key2vertex;
|
2010-01-14 11:21:07 +08:00
|
|
|
boost::tie(g, root, key2vertex) =
|
|
|
|
|
predecessorMap2Graph<PoseGraph, PoseVertex, typename Config::Key>(tree);
|
2010-01-13 00:12:25 +08:00
|
|
|
|
|
|
|
|
// 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");
|
|
|
|
|
|
|
|
|
|
// e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor
|
|
|
|
|
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
|
2010-01-13 10:09:16 +08:00
|
|
|
if (!factor) continue;
|
2010-01-13 00:12:25 +08:00
|
|
|
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
typename Config::Key key1 = factor->key1();
|
|
|
|
|
typename Config::Key key2 = factor->key2();
|
|
|
|
|
|
|
|
|
|
PoseVertex v_from = key2vertex.find(key1)->second;
|
|
|
|
|
PoseVertex v_to = key2vertex.find(key2)->second;
|
2010-01-13 00:12:25 +08:00
|
|
|
|
|
|
|
|
Pose measured = factor->measured();
|
|
|
|
|
tie(edge1, found1) = boost::edge(v_from, v_to, g);
|
|
|
|
|
tie(edge2, found2) = boost::edge(v_to, v_from, g);
|
2010-01-13 10:09:16 +08:00
|
|
|
if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree");
|
|
|
|
|
if (!found1 && !found2) continue;
|
2010-01-13 00:12:25 +08:00
|
|
|
if (found1)
|
|
|
|
|
boost::put(boost::edge_weight, g, edge1, measured);
|
|
|
|
|
else if (found2)
|
|
|
|
|
boost::put(boost::edge_weight, g, edge2, inverse(measured));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// compose poses
|
2010-01-13 10:09:16 +08:00
|
|
|
boost::shared_ptr<Config> config(new Config);
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
typename Config::Key rootKey = boost::get(boost::vertex_name, g, root);
|
|
|
|
|
config->insert(rootKey, rootPose);
|
2010-01-13 00:12:25 +08:00
|
|
|
compose_key_visitor<PoseVertex, Pose, Config> vis(config);
|
|
|
|
|
boost::breadth_first_search(g, root, boost::visitor(vis));
|
|
|
|
|
|
|
|
|
|
return config;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* ************************************************************************* */
|
|
|
|
|
|
|
|
|
|
}
|
