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

Unit test for transformPoseFrom

release/4.3a0
Frank dellaert 2020-07-21 11:37:08 -04:00
parent 2605e10c27
commit d672e7eb87
1 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/geometry/tests/testPose3.cpp
View File

@ -418,6 +418,29 @@ TEST(Pose3, transform_to_rotate) {
EXPECT(assert_equal(expected, actual, 0.001));
}
/* ************************************************************************* */
// Check transformPoseFrom and its pushforward
Pose3 transformPoseFrom_(const Pose3& wTa, const Pose3& aTb) {
return wTa.transformPoseFrom(aTb);
}
TEST(Pose3, transformPoseFrom)
{
Matrix actual = (T2*T2).matrix();
Matrix expected = T2.matrix()*T2.matrix();
EXPECT(assert_equal(actual, expected, 1e-8));
Matrix H1, H2;
T2.transformPoseFrom(T2, H1, H2);
Matrix numericalH1 = numericalDerivative21(transformPoseFrom_, T2, T2);
EXPECT(assert_equal(numericalH1, H1, 5e-3));
EXPECT(assert_equal(T2.inverse().AdjointMap(), H1, 5e-3));
Matrix numericalH2 = numericalDerivative22(transformPoseFrom_, T2, T2);
EXPECT(assert_equal(numericalH2, H2, 1e-4));
}
/* ************************************************************************* */
TEST(Pose3, transformTo) {
Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(2, 4, 0));