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

Comments, and better test of composePoses

release/4.3a0
Frank Dellaert 2010-01-27 02:49:58 +00:00
parent 8b87eebba6
commit ae073c0120
3 changed files with 22 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cpp/graph-inl.h
View File

@ -139,7 +139,7 @@ public:
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]));
config_->insert(key_to, compose((*config_)[key_from],relativePose));
}
};
@ -164,7 +164,7 @@ boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<type
predecessorMap2Graph<PoseGraph, PoseVertex, typename Config::Key>(tree);
// attach the relative poses to the edges
PoseEdge edge1, edge2;
PoseEdge edge12, edge21;
bool found1, found2;
BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) {
@ -178,18 +178,18 @@ boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<type
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;
PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->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);
Pose l1Xl2 = factor->measured();
tie(edge12, found1) = boost::edge(v1, v2, g);
tie(edge21, found2) = boost::edge(v2, v1, g);
if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree");
if (!found1 && !found2) continue;
if (found1)
boost::put(boost::edge_weight, g, edge1, measured);
boost::put(boost::edge_weight, g, edge12, l1Xl2);
else if (found2)
boost::put(boost::edge_weight, g, edge2, inverse(measured));
boost::put(boost::edge_weight, g, edge21, inverse(l1Xl2));
}
// compose poses

19
cpp/testGraph.cpp
View File

@ -72,25 +72,30 @@ TEST( Graph, composePoses )
{
Pose2Graph graph;
Matrix cov = eye(3);
graph.addConstraint(1,2, Pose2(2.0, 0.0, 0.0), cov);
graph.addConstraint(2,3, Pose2(3.0, 0.0, 0.0), cov);
Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9);
Pose2 p12=between(p1,p2), p23=between(p2,p3), p43=between(p4,p3);
graph.addConstraint(1,2, p12, cov);
graph.addConstraint(2,3, p23, cov);
graph.addConstraint(4,3, p43, cov);
PredecessorMap<Pose2Config::Key> tree;
tree.insert(1,2);
tree.insert(2,2);
tree.insert(3,2);
tree.insert(4,3);
Pose2 rootPose(3.0, 0.0, 0.0);
Pose2 rootPose = p2;
boost::shared_ptr<Pose2Config> actual = composePoses<Pose2Graph, Pose2Factor,
Pose2, Pose2Config> (graph, tree, rootPose);
Pose2Config expected;
expected.insert(1, Pose2(1.0, 0.0, 0.0));
expected.insert(2, Pose2(3.0, 0.0, 0.0));
expected.insert(3, Pose2(6.0, 0.0, 0.0));
expected.insert(1, p1);
expected.insert(2, p2);
expected.insert(3, p3);
expected.insert(4, p4);
LONGS_EQUAL(3, actual->size());
LONGS_EQUAL(4, actual->size());
CHECK(assert_equal(expected, *actual));
}

1
cpp/testVSLAMFactor.cpp
View File

@ -76,6 +76,7 @@ TEST( ProjectionFactor, error )
CHECK(assert_equal(expected_config,actual_config,1e-9));
}
/* ************************************************************************* */
TEST( ProjectionFactor, equals )
{
// Create two identical factors and make sure they're equal