Some more tests on retract/localCoordinates
parent
2c235d98be
commit
387dfe5229
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue