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

added manifold test

release/4.3a0
Frank Dellaert 2010-01-05 14:13:51 +00:00
parent 39334ea9e0
commit 22d66df766
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cpp/testPose2.cpp
View File

@ -47,6 +47,19 @@ TEST(Pose2, constructors) {
assert_equal(pose,origin);
}
/* ************************************************************************* */
TEST(Pose2, manifold) {
Pose2 t1(M_PI_2, Point2(1, 2));
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 origin;
Vector d12 = t1.log(t2);
CHECK(assert_equal(t2, t1.exmap(d12)));
CHECK(assert_equal(t2, origin.exmap(d12)*t1));
Vector d21 = t2.log(t1);
CHECK(assert_equal(t1, t2.exmap(d21)));
CHECK(assert_equal(t1, origin.exmap(d21)*t2));
}
/* ************************************************************************* */
TEST(Pose2, exmap) {
Pose2 pose(M_PI_2, Point2(1, 2));