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

return shared_ptr in composePoses

release/4.3a0
Kai Ni 2010-01-13 02:09:16 +00:00
parent 8d28763339
commit 2a094b7491
2 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cpp/graph-inl.h
View File

@ -119,16 +119,16 @@ boost::tuple<G, V, map<string, V> > predecessorMap2Graph(const map<string, strin
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) {}
compose_key_visitor(boost::shared_ptr<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));
config->insert(key_to, compose(config->get(key_from), relativePose));
}
private:
PoseConfig& config;
boost::shared_ptr<PoseConfig> config;
};
@ -137,7 +137,7 @@ private:
* 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,
boost::shared_ptr<Config> composePoses(const G& graph, const map<string, string>& tree,
const Pose& rootPose) {
//TODO: change edge_weight_t to edge_pose_t
@ -160,10 +160,10 @@ Config composePoses(const G& graph, const map<string, string>& tree,
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);
if (!factor) continue;
PoseVertex v_from = key2vertex.find(factor->keys().front())->second;
PoseVertex v_to = key2vertex.find(factor->keys().back())->second;
@ -171,8 +171,8 @@ Config composePoses(const G& graph, const map<string, string>& tree,
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 && found2) throw invalid_argument ("composePoses: invalid spanning tree");
if (!found1 && !found2) continue;
if (found1)
boost::put(boost::edge_weight, g, edge1, measured);
else if (found2)
@ -180,8 +180,8 @@ Config composePoses(const G& graph, const map<string, string>& tree,
}
// compose poses
Config config;
config.insert(boost::get(boost::vertex_name, g, root), rootPose);
boost::shared_ptr<Config> config(new 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));

7
cpp/testGraph.cpp
View File

@ -34,14 +34,15 @@ TEST( Graph, composePoses )
Pose2 rootPose(3.0, 0.0, 0.0);
Pose2Config actual = composePoses<Pose2Graph, Pose2Factor, Pose2, Pose2Config>(graph, tree, rootPose);
boost::shared_ptr<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));
LONGS_EQUAL(3, actual->size());
CHECK(assert_equal(expected, *actual));
}
/* ************************************************************************* */