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

failing test

release/4.3a0
Frank Dellaert 2010-01-22 05:45:59 +00:00
parent 5367e5a157
commit 75e29dc015
1 changed files with 69 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
cpp/testPose2.cpp
View File

@ -148,6 +148,62 @@ TEST(Pose2, compose_c)
CHECK(assert_equal(pose_expected, pose_actual_fcn));
}
/* ************************************************************************* */
TEST(Pose2, inverse )
{
Point2 origin, t(1,2);
Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y
Pose2 identity, lTg = inverse(gTl);
CHECK(assert_equal(identity,compose(lTg,gTl)));
CHECK(assert_equal(identity,compose(gTl,lTg)));
Point2 l(4,5), g(-4,6);
CHECK(assert_equal(g,gTl*l));
CHECK(assert_equal(l,lTg*g));
}
/* ************************************************************************* */
Vector homogeneous(const Point2& p) {
return Vector_(3, p.x(), p.y(), 1.0);
}
Matrix matrix(const Pose2& gTl) {
Matrix gRl = gTl.r().matrix();
Point2 gt = gTl.t();
return Matrix_(3, 3,
gRl(0, 0), gRl(0, 1), gt.x(),
gRl(1, 0), gRl(1, 1), gt.y(),
0.0, 0.0, 1.0);
}
TEST( Pose2, matrix )
{
Point2 origin, t(1,2);
Pose2 gT1(M_PI_2, t); // robot at (1,2) looking towards y
Matrix gM1 = matrix(gT1);
CHECK(assert_equal(Matrix_(3,3,
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0),
gM1));
Rot2 gR1 = gT1.r();
CHECK(assert_equal(homogeneous(t),gM1*homogeneous(origin)));
Point2 x_axis(1,0), y_axis(0,1);
CHECK(assert_equal(Matrix_(2,2,
0.0, -1.0,
1.0, 0.0),
gR1.matrix()));
CHECK(assert_equal(Point2(0,1),gR1*x_axis));
CHECK(assert_equal(Point2(-1,0),gR1*y_axis));
CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gM1*homogeneous(x_axis)));
CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gM1*homogeneous(y_axis)));
// check inverse pose
Matrix _1Mg = matrix(inverse(gT1));
CHECK(assert_equal(inverse(gM1),_1Mg));
}
/* ************************************************************************* */
TEST( Pose2, between )
@ -157,13 +213,20 @@ TEST( Pose2, between )
// ^
//
// *--0--*--*
Pose2 p1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 p2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Matrix actualH1,actualH2;
Pose2 expected(M_PI_2, Point2(2,2));
Pose2 actual1 = between(p1,p2);
Pose2 actual2 = between(p1,p2,actualH1,actualH2);
Pose2 actual1 = between(gT1,gT2); // gT2 * 1Tg does not make sense !!!!!
GTSAM_PRINT(between(gT1,gT2));
// what we want is 1T2 = 1Tg * gT2 = between(2Tg,1Tg)
// print(matrix(gT1));
// print(matrix(gT2));
// print(inverse(matrix(gT1))*matrix(gT2)); // 1T2
// print(inverse(matrix(gT2))*matrix(gT1)); // 2T1
// GTSAM_PRINT(between(inverse(gT2),inverse(gT1)));
Pose2 actual2 = between(gT1,gT2,actualH1,actualH2);
CHECK(assert_equal(expected,actual1));
CHECK(assert_equal(expected,actual2));
@ -172,7 +235,7 @@ TEST( Pose2, between )
1.0, 0.0,-2.0,
0.0, 0.0,-1.0
);
Matrix numericalH1 = numericalDerivative21(between<Pose2>, p1, p2, 1e-5);
Matrix numericalH1 = numericalDerivative21(between<Pose2>, gT1, gT2, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(numericalH1,actualH1));
@ -181,7 +244,7 @@ TEST( Pose2, between )
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
);
Matrix numericalH2 = numericalDerivative22(between<Pose2>, p1, p2, 1e-5);
Matrix numericalH2 = numericalDerivative22(between<Pose2>, gT1, gT2, 1e-5);
CHECK(assert_equal(expectedH2,actualH2));
CHECK(assert_equal(numericalH2,actualH2));
}