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

Some more tests on retract/localCoordinates

release/4.3a0
dellaert 2015-05-24 15:33:15 -07:00
parent 2c235d98be
commit 387dfe5229
1 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,6 +30,14 @@ TEST (EssentialMatrix, equality) {
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
TEST (EssentialMatrix, FromPose3) {
EssentialMatrix expected(c1Rc2, Unit3(c1Tc2));
Pose3 pose(c1Rc2, c1Tc2);
EssentialMatrix actual = EssentialMatrix::FromPose3(pose);
EXPECT(assert_equal(expected, actual));
}
//*******************************************************************************
TEST(EssentialMatrix, localCoordinates0) {
EssentialMatrix E;
@ -47,11 +55,11 @@ TEST (EssentialMatrix, localCoordinates) {
Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
EXPECT(assert_equal(zero(5), actual, 1e-8));
Vector d = zero(6);
d(4) += 1e-5;
Vector6 d;
d << 0.1, 0.2, 0.3, 0, 0, 0;
Vector actual2 = hx.localCoordinates(
EssentialMatrix::FromPose3(pose.retract(d)));
EXPECT(assert_equal(zero(5), actual2, 1e-8));
EXPECT(assert_equal(d.head(5), actual2, 1e-8));
}
//*************************************************************************
@ -75,6 +83,15 @@ TEST (EssentialMatrix, retract2) {
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
TEST (EssentialMatrix, RoundTrip) {
Vector5 d;
d << 0.1, 0.2, 0.3, 0.4, 0.5;
EssentialMatrix e, hx = e.retract(d);
Vector actual = e.localCoordinates(hx);
EXPECT(assert_equal(d, actual, 1e-8));
}
//*************************************************************************
Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
return E.transform_to(point);