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

Another unit test for between derivatives

release/4.3a0
Frank Dellaert 2013-04-05 00:11:19 +00:00
parent 5548d78071
commit 6162ce6fa1
1 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/tests/testPose2.cpp
View File

@ -457,6 +457,21 @@ TEST( Pose2, between2 )
EXPECT(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
// arbitrary, non perpendicular angles to be extra safe
TEST( Pose2, between3 )
{
Pose2 p2(M_PI/3.0, Point2(1,2));
Pose2 p1(M_PI/6.0, Point2(-1,4));
Matrix actualH1,actualH2;
p1.between(p2,actualH1,actualH2);
Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2);
EXPECT(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Pose2, round_trip )
{