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

I think we might have a problem: is expmap<Pose2> correct? Uncomment the new test in testPose2. It does not match what is said in http://www.cis.upenn.edu/~cis610/cis610lie1.ps.

release/4.3a0
Frank Dellaert 2010-02-26 06:00:58 +00:00
parent 0004d90feb
commit c40760485d
1 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cpp/testPose2.cpp
View File

@ -41,11 +41,27 @@ TEST(Pose2, manifold) {
TEST(Pose2, expmap) { TEST(Pose2, expmap) {
//cout << "expmap" << endl; //cout << "expmap" << endl;
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 expected(M_PI_2+0.99, Point2(1.015, 2.01));
Pose2 actual = expmap(pose, Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = expmap(pose, Vector_(3, 0.01, -0.015, 0.99));
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */
TEST(Pose2, expmap2) {
// do an actual series exponential map
// see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps
Matrix A = Matrix_(3,3,
0.0, -0.99, 0.01,
0.99, 0.0, -0.015,
0.0, 0.0, 0.0);
Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
Matrix expected = eye(3) + A + A2 + A3 + A4;
Pose2 pose = expmap<Pose2>(Vector_(3, 0.01, -0.015, 0.99));
Matrix actual = pose.matrix();
//CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap0) { TEST(Pose2, expmap0) {
//cout << "expmap0" << endl; //cout << "expmap0" << endl;