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

fix a bug in composePoses that switched the order of compose parameters

release/4.3a0
Kai Ni 2010-01-13 06:02:18 +00:00
parent 2a094b7491
commit c0bc7e6c10
2 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/graph-inl.h
View File

@ -124,7 +124,7 @@ public:
string key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); 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)); string key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
Pose relativePose = boost::get(boost::edge_weight, g, edge); Pose relativePose = boost::get(boost::edge_weight, g, edge);
config->insert(key_to, compose(config->get(key_from), relativePose)); config->insert(key_to, compose(relativePose, config->get(key_from)));
} }
private: private:

9
cpp/testPose2.cpp
View File

@ -181,6 +181,15 @@ TEST( Pose2, between )
CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2));
} }
/* ************************************************************************* */
TEST( Pose2, round_trip )
{
Pose2 p1(1.23, 2.30, 0.2);
Pose2 odo(0.53, 0.39, 0.15);
Pose2 p2 = compose(odo, p1);
CHECK(assert_equal(odo, between(p1, p2)));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, members) TEST(Pose2, members)
{ {