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

Merge pull request #183 from borglab/fix/unit-test-tolerances 

Enable and fix unit tests in different archs
release/4.3a0
Frank Dellaert 2020-05-09 16:41:44 -04:00 committed by GitHub
parent 3e6d360ff8 2622acb7a7
commit f948fe3442
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 98 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
debian/rules vendored
View File

@ -3,6 +3,8 @@
# output every command that modifies files on the build system. # output every command that modifies files on the build system.
export DH_VERBOSE = 1 export DH_VERBOSE = 1
# Makefile target name for running unit tests:
GTSAM_TEST_TARGET = check
# see FEATURE AREAS in dpkg-buildflags(1) # see FEATURE AREAS in dpkg-buildflags(1)
#export DEB_BUILD_MAINT_OPTIONS = hardening=+all #export DEB_BUILD_MAINT_OPTIONS = hardening=+all
@ -13,13 +15,15 @@ export DH_VERBOSE = 1
# package maintainers to append LDFLAGS # package maintainers to append LDFLAGS
#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed #export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed
%: %:
dh $@ --parallel dh $@ --parallel
# dh_make generated override targets # dh_make generated override targets
# This is example for Cmake (See https://bugs.debian.org/641051 ) # This is example for Cmake (See https://bugs.debian.org/641051 )
override_dh_auto_configure: override_dh_auto_configure:
dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=ON -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF
override_dh_auto_test-arch:
# Tests for arch-dependent :
echo "[override_dh_auto_test-arch]"
dh_auto_build -O--buildsystem=cmake -- $(GTSAM_TEST_TARGET)

20
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -66,8 +66,8 @@ TEST (OrientedPlane3, transform) {
OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose, OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose,
none, none); none, none);
OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none); OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none);
EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-9)); EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-5));
EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-9)); EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-5));
// Test the jacobians of transform // Test the jacobians of transform
Matrix actualH1, expectedH1, actualH2, expectedH2; Matrix actualH1, expectedH1, actualH2, expectedH2;
@ -75,18 +75,18 @@ TEST (OrientedPlane3, transform) {
// because the Transform class uses a wrong order of Jacobians in interface // because the Transform class uses a wrong order of Jacobians in interface
OrientedPlane3::Transform(plane, pose, actualH1, none); OrientedPlane3::Transform(plane, pose, actualH1, none);
expectedH1 = numericalDerivative22(Transform_, plane, pose); expectedH1 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
OrientedPlane3::Transform(plane, pose, none, actualH2); OrientedPlane3::Transform(plane, pose, none, actualH2);
expectedH2 = numericalDerivative21(Transform_, plane, pose); expectedH2 = numericalDerivative21(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
} }
{ {
plane.transform(pose, actualH1, none); plane.transform(pose, actualH1, none);
expectedH1 = numericalDerivative21(transform_, plane, pose); expectedH1 = numericalDerivative21(transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
plane.transform(pose, none, actualH2); plane.transform(pose, none, actualH2);
expectedH2 = numericalDerivative22(Transform_, plane, pose); expectedH2 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
} }
} }
@ -157,8 +157,8 @@ TEST (OrientedPlane3, error2) {
boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none); boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
expectedH1 = numericalDerivative21(f, plane1, plane2); expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2); expectedH2 = numericalDerivative22(f, plane1, plane2);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
} }
//******************************************************************************* //*******************************************************************************
@ -171,13 +171,13 @@ TEST (OrientedPlane3, jacobian_retract) {
Vector3 v (-0.1, 0.2, 0.3); Vector3 v (-0.1, 0.2, 0.3);
plane.retract(v, H_actual); plane.retract(v, H_actual);
Matrix H_expected_numerical = numericalDerivative11(f, v); Matrix H_expected_numerical = numericalDerivative11(f, v);
EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9)); EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));
} }
{ {
Vector3 v (0, 0, 0); Vector3 v (0, 0, 0);
plane.retract(v, H_actual); plane.retract(v, H_actual);
Matrix H_expected_numerical = numericalDerivative11(f, v); Matrix H_expected_numerical = numericalDerivative11(f, v);
EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9)); EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));
} }
} }

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

@ -649,8 +649,8 @@ TEST(Pose3, Bearing) {
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x1, l1); expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
expectedH2 = numericalDerivative22(bearing_proxy, x1, l1); expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
EXPECT(assert_equal(expectedH1, actualH1)); EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
EXPECT(assert_equal(expectedH2, actualH2)); EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
} }
TEST(Pose3, Bearing2) { TEST(Pose3, Bearing2) {
@ -660,8 +660,8 @@ TEST(Pose3, Bearing2) {
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x2, l4); expectedH1 = numericalDerivative21(bearing_proxy, x2, l4);
expectedH2 = numericalDerivative22(bearing_proxy, x2, l4); expectedH2 = numericalDerivative22(bearing_proxy, x2, l4);
EXPECT(assert_equal(expectedH1, actualH1)); EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
EXPECT(assert_equal(expectedH2, actualH2)); EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
} }
TEST(Pose3, PoseToPoseBearing) { TEST(Pose3, PoseToPoseBearing) {
@ -681,8 +681,8 @@ TEST(Pose3, PoseToPoseBearing) {
expectedH2.setZero(); expectedH2.setZero();
expectedH2.block<2, 3>(0, 3) = H2block; expectedH2.block<2, 3>(0, 3) = H2block;
EXPECT(assert_equal(expectedH1, actualH1)); EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
EXPECT(assert_equal(expectedH2, actualH2)); EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */

106
gtsam/geometry/tests/testUnit3.cpp
View File

@ -58,8 +58,8 @@ TEST(Unit3, point3) {
for(Point3 p: ps) { for(Point3 p: ps) {
Unit3 s(p); Unit3 s(p);
expectedH = numericalDerivative11<Point3, Unit3>(point3_, s); expectedH = numericalDerivative11<Point3, Unit3>(point3_, s);
EXPECT(assert_equal(p, s.point3(actualH), 1e-8)); EXPECT(assert_equal(p, s.point3(actualH), 1e-5));
EXPECT(assert_equal(expectedH, actualH, 1e-9)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
} }
@ -73,18 +73,18 @@ TEST(Unit3, rotate) {
Unit3 p(1, 0, 0); Unit3 p(1, 0, 0);
Unit3 expected = Unit3(R.column(1)); Unit3 expected = Unit3(R.column(1));
Unit3 actual = R * p; Unit3 actual = R * p;
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-5));
Matrix actualH, expectedH; Matrix actualH, expectedH;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expectedH = numericalDerivative21(rotate_, R, p); expectedH = numericalDerivative21(rotate_, R, p);
R.rotate(p, actualH, boost::none); R.rotate(p, actualH, boost::none);
EXPECT(assert_equal(expectedH, actualH, 1e-9)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
{ {
expectedH = numericalDerivative22(rotate_, R, p); expectedH = numericalDerivative22(rotate_, R, p);
R.rotate(p, boost::none, actualH); R.rotate(p, boost::none, actualH);
EXPECT(assert_equal(expectedH, actualH, 1e-9)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
} }
@ -98,19 +98,19 @@ TEST(Unit3, unrotate) {
Unit3 p(1, 0, 0); Unit3 p(1, 0, 0);
Unit3 expected = Unit3(1, 1, 0); Unit3 expected = Unit3(1, 1, 0);
Unit3 actual = R.unrotate(p); Unit3 actual = R.unrotate(p);
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-5));
Matrix actualH, expectedH; Matrix actualH, expectedH;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expectedH = numericalDerivative21(unrotate_, R, p); expectedH = numericalDerivative21(unrotate_, R, p);
R.unrotate(p, actualH, boost::none); R.unrotate(p, actualH, boost::none);
EXPECT(assert_equal(expectedH, actualH, 1e-9)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
{ {
expectedH = numericalDerivative22(unrotate_, R, p); expectedH = numericalDerivative22(unrotate_, R, p);
R.unrotate(p, boost::none, actualH); R.unrotate(p, boost::none, actualH);
EXPECT(assert_equal(expectedH, actualH, 1e-9)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
} }
@ -119,7 +119,7 @@ TEST(Unit3, dot) {
Unit3 q = p.retract(Vector2(0.5, 0)); Unit3 q = p.retract(Vector2(0.5, 0));
Unit3 r = p.retract(Vector2(0.8, 0)); Unit3 r = p.retract(Vector2(0.8, 0));
Unit3 t = p.retract(Vector2(0, 0.3)); Unit3 t = p.retract(Vector2(0, 0.3));
EXPECT(assert_equal(1.0, p.dot(p), 1e-8)); EXPECT(assert_equal(1.0, p.dot(p), 1e-5));
EXPECT(assert_equal(0.877583, p.dot(q), 1e-5)); EXPECT(assert_equal(0.877583, p.dot(q), 1e-5));
EXPECT(assert_equal(0.696707, p.dot(r), 1e-5)); EXPECT(assert_equal(0.696707, p.dot(r), 1e-5));
EXPECT(assert_equal(0.955336, p.dot(t), 1e-5)); EXPECT(assert_equal(0.955336, p.dot(t), 1e-5));
@ -130,18 +130,18 @@ TEST(Unit3, dot) {
boost::none, boost::none); boost::none, boost::none);
{ {
p.dot(q, H1, H2); p.dot(q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, q), H2, 1e-9)); EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, q), H2, 1e-5));
} }
{ {
p.dot(r, H1, H2); p.dot(r, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, r), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, r), H1, 1e-5));
EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, r), H2, 1e-9)); EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, r), H2, 1e-5));
} }
{ {
p.dot(t, H1, H2); p.dot(t, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, t), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, t), H1, 1e-5));
EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, t), H2, 1e-9)); EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, t), H2, 1e-5));
} }
} }
@ -149,7 +149,7 @@ TEST(Unit3, dot) {
TEST(Unit3, error) { TEST(Unit3, error) {
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
r = p.retract(Vector2(0.8, 0)); r = p.retract(Vector2(0.8, 0));
EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8)); EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5)); EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
@ -159,13 +159,13 @@ TEST(Unit3, error) {
expected = numericalDerivative11<Vector2,Unit3>( expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q); boost::bind(&Unit3::error, &p, _1, boost::none), q);
p.error(q, actual); p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
} }
{ {
expected = numericalDerivative11<Vector2,Unit3>( expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r); boost::bind(&Unit3::error, &p, _1, boost::none), r);
p.error(r, actual); p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
} }
} }
@ -176,7 +176,7 @@ TEST(Unit3, error2) {
Unit3 r = p.retract(Vector2(0.8, 0)); Unit3 r = p.retract(Vector2(0.8, 0));
// Hard-coded as simple regression values // Hard-coded as simple regression values
EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8)); EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5)); EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5)); EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5));
@ -186,25 +186,25 @@ TEST(Unit3, error2) {
expected = numericalDerivative21<Vector2, Unit3, Unit3>( expected = numericalDerivative21<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
p.errorVector(q, actual, boost::none); p.errorVector(q, actual, boost::none);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
{ {
expected = numericalDerivative21<Vector2, Unit3, Unit3>( expected = numericalDerivative21<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
p.errorVector(r, actual, boost::none); p.errorVector(r, actual, boost::none);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
{ {
expected = numericalDerivative22<Vector2, Unit3, Unit3>( expected = numericalDerivative22<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
p.errorVector(q, boost::none, actual); p.errorVector(q, boost::none, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
{ {
expected = numericalDerivative22<Vector2, Unit3, Unit3>( expected = numericalDerivative22<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
p.errorVector(r, boost::none, actual); p.errorVector(r, boost::none, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
} }
@ -212,9 +212,9 @@ TEST(Unit3, error2) {
TEST(Unit3, distance) { TEST(Unit3, distance) {
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
r = p.retract(Vector2(0.8, 0)); r = p.retract(Vector2(0.8, 0));
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-5);
EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8); EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-5);
EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8); EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-5);
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
@ -222,13 +222,13 @@ TEST(Unit3, distance) {
expected = numericalGradient<Unit3>( expected = numericalGradient<Unit3>(
boost::bind(&Unit3::distance, &p, _1, boost::none), q); boost::bind(&Unit3::distance, &p, _1, boost::none), q);
p.distance(q, actual); p.distance(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
} }
{ {
expected = numericalGradient<Unit3>( expected = numericalGradient<Unit3>(
boost::bind(&Unit3::distance, &p, _1, boost::none), r); boost::bind(&Unit3::distance, &p, _1, boost::none), r);
p.distance(r, actual); p.distance(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
} }
} }
@ -236,7 +236,7 @@ TEST(Unit3, distance) {
TEST(Unit3, localCoordinates0) { TEST(Unit3, localCoordinates0) {
Unit3 p; Unit3 p;
Vector actual = p.localCoordinates(p); Vector actual = p.localCoordinates(p);
EXPECT(assert_equal(Z_2x1, actual, 1e-8)); EXPECT(assert_equal(Z_2x1, actual, 1e-5));
} }
TEST(Unit3, localCoordinates) { TEST(Unit3, localCoordinates) {
@ -244,46 +244,46 @@ TEST(Unit3, localCoordinates) {
Unit3 p, q; Unit3 p, q;
Vector2 expected = Vector2::Zero(); Vector2 expected = Vector2::Zero();
Vector2 actual = p.localCoordinates(q); Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-5));
EXPECT(assert_equal(q, p.retract(expected), 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-5));
} }
{ {
Unit3 p, q(1, 6.12385e-21, 0); Unit3 p, q(1, 6.12385e-21, 0);
Vector2 expected = Vector2::Zero(); Vector2 expected = Vector2::Zero();
Vector2 actual = p.localCoordinates(q); Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-5));
EXPECT(assert_equal(q, p.retract(expected), 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-5));
} }
{ {
Unit3 p, q(-1, 0, 0); Unit3 p, q(-1, 0, 0);
Vector2 expected(M_PI, 0); Vector2 expected(M_PI, 0);
Vector2 actual = p.localCoordinates(q); Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-5));
EXPECT(assert_equal(q, p.retract(expected), 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-5));
} }
{ {
Unit3 p, q(0, 1, 0); Unit3 p, q(0, 1, 0);
Vector2 expected(0,-M_PI_2); Vector2 expected(0,-M_PI_2);
Vector2 actual = p.localCoordinates(q); Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-5));
EXPECT(assert_equal(q, p.retract(expected), 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-5));
} }
{ {
Unit3 p, q(0, -1, 0); Unit3 p, q(0, -1, 0);
Vector2 expected(0, M_PI_2); Vector2 expected(0, M_PI_2);
Vector2 actual = p.localCoordinates(q); Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-5));
EXPECT(assert_equal(q, p.retract(expected), 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-5));
} }
{ {
Unit3 p(0,1,0), q(0,-1,0); Unit3 p(0,1,0), q(0,-1,0);
Vector2 actual = p.localCoordinates(q); Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(q, p.retract(actual), 1e-8)); EXPECT(assert_equal(q, p.retract(actual), 1e-5));
} }
{ {
Unit3 p(0,0,1), q(0,0,-1); Unit3 p(0,0,1), q(0,0,-1);
Vector2 actual = p.localCoordinates(q); Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(q, p.retract(actual), 1e-8)); EXPECT(assert_equal(q, p.retract(actual), 1e-5));
} }
double twist = 1e-4; double twist = 1e-4;
@ -328,11 +328,11 @@ TEST(Unit3, basis) {
// with H, first time // with H, first time
EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
// with H, cached // with H, cached
EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
//******************************************************************************* //*******************************************************************************
@ -348,7 +348,7 @@ TEST(Unit3, basis_derivatives) {
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>( Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
boost::bind(BasisTest, _1, boost::none), p); boost::bind(BasisTest, _1, boost::none), p);
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
} }
@ -360,14 +360,14 @@ TEST(Unit3, retract) {
Unit3 expected(0.877583, 0, 0.479426); Unit3 expected(0.877583, 0, 0.479426);
Unit3 actual = p.retract(v); Unit3 actual = p.retract(v);
EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5));
} }
{ {
Unit3 p; Unit3 p;
Vector2 v(0, 0); Vector2 v(0, 0);
Unit3 actual = p.retract(v); Unit3 actual = p.retract(v);
EXPECT(assert_equal(p, actual, 1e-6)); EXPECT(assert_equal(p, actual, 1e-6));
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5));
} }
} }
@ -381,13 +381,13 @@ TEST (Unit3, jacobian_retract) {
Vector2 v (-0.2, 0.1); Vector2 v (-0.2, 0.1);
p.retract(v, H); p.retract(v, H);
Matrix H_expected_numerical = numericalDerivative11(f, v); Matrix H_expected_numerical = numericalDerivative11(f, v);
EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); EXPECT(assert_equal(H_expected_numerical, H, 1e-5));
} }
{ {
Vector2 v (0, 0); Vector2 v (0, 0);
p.retract(v, H); p.retract(v, H);
Matrix H_expected_numerical = numericalDerivative11(f, v); Matrix H_expected_numerical = numericalDerivative11(f, v);
EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); EXPECT(assert_equal(H_expected_numerical, H, 1e-5));
} }
} }
@ -397,8 +397,8 @@ TEST(Unit3, retract_expmap) {
Vector2 v((M_PI / 2.0), 0); Vector2 v((M_PI / 2.0), 0);
Unit3 expected(Point3(0, 0, 1)); Unit3 expected(Point3(0, 0, 1));
Unit3 actual = p.retract(v); Unit3 actual = p.retract(v);
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-5));
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5));
} }
//******************************************************************************* //*******************************************************************************
@ -428,7 +428,7 @@ TEST(Unit3, localCoordinates_retract) {
// Check if the local coordinates and retract return consistent results. // Check if the local coordinates and retract return consistent results.
Vector v12 = s1.localCoordinates(s2); Vector v12 = s1.localCoordinates(s2);
Unit3 actual_s2 = s1.retract(v12); Unit3 actual_s2 = s1.retract(v12);
EXPECT(assert_equal(s2, actual_s2, 1e-9)); EXPECT(assert_equal(s2, actual_s2, 1e-5));
} }
} }
@ -437,10 +437,10 @@ TEST (Unit3, FromPoint3) {
Matrix actualH; Matrix actualH;
Point3 point(1, -2, 3); // arbitrary point Point3 point(1, -2, 3); // arbitrary point
Unit3 expected(point); Unit3 expected(point);
EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-8)); EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5));
Matrix expectedH = numericalDerivative11<Unit3, Point3>( Matrix expectedH = numericalDerivative11<Unit3, Point3>(
boost::bind(Unit3::FromPoint3, _1, boost::none), point); boost::bind(Unit3::FromPoint3, _1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
//******************************************************************************* //*******************************************************************************

44
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -159,7 +159,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
factor.computeJacobians(Fs, E, b, cameras, *point); factor.computeJacobians(Fs, E, b, cameras, *point);
double actualError3 = b.squaredNorm(); double actualError3 = b.squaredNorm();
EXPECT(assert_equal(expectedE, E, 1e-7)); EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8); EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
} }
/* *************************************************************************/ /* *************************************************************************/
@ -329,7 +329,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-8)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
} }
/* *************************************************************************/ /* *************************************************************************/
@ -407,10 +407,10 @@ TEST( SmartProjectionPoseFactor, Factors ) {
boost::shared_ptr<RegularHessianFactor<6> > actual = boost::shared_ptr<RegularHessianFactor<6> > actual =
smartFactor1->createHessianFactor(cameras, 0.0); smartFactor1->createHessianFactor(cameras, 0.0);
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
EXPECT(assert_equal(expected, *actual, 1e-8)); EXPECT(assert_equal(expected, *actual, 1e-6));
EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
} }
{ {
@ -448,15 +448,15 @@ TEST( SmartProjectionPoseFactor, Factors ) {
SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
Matrix3 P = (E.transpose() * E).inverse(); Matrix3 P = (E.transpose() * E).inverse();
JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n);
EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8)); EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6));
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ = boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
smartFactor1->createJacobianQFactor(cameras, 0.0); smartFactor1->createJacobianQFactor(cameras, 0.0);
CHECK(actualQ); CHECK(actualQ);
EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8)); EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6));
EXPECT(assert_equal(expectedQ, *actualQ)); EXPECT(assert_equal(expectedQ, *actualQ));
EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-8); EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6);
EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-8); EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6);
// Whiten for RegularImplicitSchurFactor (does not have noise model) // Whiten for RegularImplicitSchurFactor (does not have noise model)
model->WhitenSystem(E, b); model->WhitenSystem(E, b);
@ -470,11 +470,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual = boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
CHECK(actual); CHECK(actual);
EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
EXPECT(assert_equal(expected, *actual)); EXPECT(assert_equal(expected, *actual));
EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
} }
{ {
@ -484,15 +484,15 @@ TEST( SmartProjectionPoseFactor, Factors ) {
double s = sigma * sin(M_PI_4); double s = sigma * sin(M_PI_4);
SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma);
JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); JacobianFactor expected(x1, s * A1, x2, s * A2, b, n);
EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));
boost::shared_ptr<JacobianFactor> actual = boost::shared_ptr<JacobianFactor> actual =
smartFactor1->createJacobianSVDFactor(cameras, 0.0); smartFactor1->createJacobianSVDFactor(cameras, 0.0);
CHECK(actual); CHECK(actual);
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
EXPECT(assert_equal(expected, *actual)); EXPECT(assert_equal(expected, *actual));
EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
} }
} }
@ -603,7 +603,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-8)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
} }
/* *************************************************************************/ /* *************************************************************************/
@ -779,7 +779,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-8)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
} }
/* *************************************************************************/ /* *************************************************************************/
@ -899,7 +899,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
Matrix GraphInformation = GaussianGraph->hessian().first; Matrix GraphInformation = GaussianGraph->hessian().first;
// Check Hessian // Check Hessian
EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));
Matrix AugInformationMatrix = factor1->augmentedInformation() Matrix AugInformationMatrix = factor1->augmentedInformation()
+ factor2->augmentedInformation() + factor3->augmentedInformation(); + factor2->augmentedInformation() + factor3->augmentedInformation();
@ -908,7 +908,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
// Check Hessian // Check Hessian
EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
} }
/* *************************************************************************/ /* *************************************************************************/