From 1a94808f57e4f7bbddcdcfda1404940c3134eabd Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 8 Apr 2011 18:41:50 +0000 Subject: [PATCH] Tightened thresholds in Rot3, Pose3, Rot2, Pose2, that were "correcting" numbers to zero and one with too loose tolerance. This made it necessary to specify large steps for numerical derivatives (1e-2 instead of the default 1e-5). Fixed this, and updated unit tests to use the default step size. --- gtsam/geometry/Pose2.cpp | 6 ++-- gtsam/geometry/Pose3.cpp | 4 +-- gtsam/geometry/Rot2.cpp | 2 +- gtsam/geometry/Rot3.cpp | 12 ++++---- gtsam/geometry/tests/testPose2.cpp | 46 ++++++++++++++-------------- gtsam/geometry/tests/testPose3.cpp | 24 +++++++-------- gtsam/geometry/tests/testRot2.cpp | 4 +-- gtsam/geometry/tests/testRot3.cpp | 6 ++-- gtsam/slam/tests/testPose2Factor.cpp | 4 +-- gtsam/slam/tests/testPose2Prior.cpp | 2 +- 10 files changed, 55 insertions(+), 55 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 6a87d3d6b..751fc8bf5 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -51,7 +51,7 @@ namespace gtsam { assert(xi.size() == 3); Point2 v(xi(0),xi(1)); double w = xi(2); - if (fabs(w) < 1e-5) + if (fabs(w) < 1e-10) return Pose2(xi[0], xi[1], xi[2]); else { Rot2 R(Rot2::fromAngle(w)); @@ -66,7 +66,7 @@ namespace gtsam { const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); - if (fabs(w) < 1e-5) + if (fabs(w) < 1e-10) return Vector_(3, t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); @@ -216,7 +216,7 @@ namespace gtsam { Point2 d = transform_to(point, H1, H2); double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); Matrix D_result_d; - if(fabs(n) > 1e-5) + if(fabs(n) > 1e-10) D_result_d = Matrix_(1, 2, x / n, y / n); else { D_result_d = Matrix_(1,2, 1.0, 1.0); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index a6abef1f8..0d6e70b31 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -60,7 +60,7 @@ namespace gtsam { Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5)); double theta = w.norm(); - if (theta < 1e-5) { + if (theta < 1e-10) { static const Rot3 I; return Pose3(I, v); } @@ -78,7 +78,7 @@ namespace gtsam { Vector Pose3::LogmapFull(const Pose3& p) { Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = norm_2(w); - if (t < 1e-5) + if (t < 1e-10) return concatVectors(2, &w, &T); else { Matrix W = skewSymmetric(w/t); diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index f7aebbed7..e150af203 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -55,7 +55,7 @@ bool Rot2::equals(const Rot2& R, double tol) const { /* ************************************************************************* */ Rot2& Rot2::normalize() { double scale = c_*c_ + s_*s_; - if(fabs(scale-1.0)>1e-9) { + if(fabs(scale-1.0)>1e-10) { scale = pow(scale, -0.5); c_ *= scale; s_ *= scale; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 3fbc14463..167645b37 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -105,7 +105,7 @@ namespace gtsam { /* ************************************************************************* */ Rot3 Rot3::rodriguez(const Vector& w) { double t = norm_2(w); - if (t < 1e-5) return Rot3(); + if (t < 1e-10) return Rot3(); return rodriguez(w/t, t); } @@ -156,16 +156,16 @@ namespace gtsam { // Log map at identity - return the canonical coordinates of this rotation Vector Rot3::Logmap(const Rot3& R) { double tr = R.r1().x()+R.r2().y()+R.r3().z(); - if (fabs(tr-3.0) < 1e-5) { // when theta = 0, +-2pi, +-4pi, etc. + if (fabs(tr-3.0) < 1e-10) { // when theta = 0, +-2pi, +-4pi, etc. return zero(3); - } else if (fabs(tr - -1.0) < 1e-5) { // when theta = +-pi, +-3pi, +-5pi, etc. - if(R.r3().z() != -1.0) + } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. + if(fabs(R.r3().z() - -1.0) > 1e-10) return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); - else if(R.r2().y() != -1.0) + else if(fabs(R.r2().y() - -1.0) > 1e-10) return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); - else // if(R.r1().x() != -1.0) TODO: fix this? + else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); } else { diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 9d283dd86..200a2e2ff 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -204,11 +204,11 @@ TEST( Pose2, transform_to ) EXPECT(assert_equal(expected,actual)); EXPECT(assert_equal(expectedH1,actualH1)); - Matrix numericalH1 = numericalDerivative21(transform_to_proxy, pose, point, 1e-5); + Matrix numericalH1 = numericalDerivative21(transform_to_proxy, pose, point); EXPECT(assert_equal(numericalH1,actualH1)); EXPECT(assert_equal(expectedH2,actualH2)); - Matrix numericalH2 = numericalDerivative22(transform_to_proxy, pose, point, 1e-5); + Matrix numericalH2 = numericalDerivative22(transform_to_proxy, pose, point); EXPECT(assert_equal(numericalH2,actualH2)); } @@ -230,11 +230,11 @@ TEST (Pose2, transform_from) Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.); Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.); - Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt, 1e-5); + Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt); EXPECT(assert_equal(H1_expected, H1)); EXPECT(assert_equal(H1_expected, numericalH1)); - Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt, 1e-5); + Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt); EXPECT(assert_equal(H2_expected, H2)); EXPECT(assert_equal(H2_expected, numericalH2)); } @@ -258,8 +258,8 @@ TEST(Pose2, compose_a) 0.0, 0.0, 1.0 ); Matrix expectedH2 = eye(3); - Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2, 1e-5); - Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2); + Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2); EXPECT(assert_equal(expectedH1,actualDcompose1)); EXPECT(assert_equal(numericalH1,actualDcompose1)); EXPECT(assert_equal(expectedH2,actualDcompose2)); @@ -285,8 +285,8 @@ TEST(Pose2, compose_b) Matrix actualDcompose1, actualDcompose2; Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2); - Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2, 1e-5); - Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2); + Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2); EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5)); EXPECT(assert_equal(numericalH2,actualDcompose2)); @@ -306,8 +306,8 @@ TEST(Pose2, compose_c) Matrix actualDcompose1, actualDcompose2; Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2); - Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2, 1e-5); - Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2); + Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2); EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5)); EXPECT(assert_equal(numericalH2,actualDcompose2)); @@ -330,7 +330,7 @@ TEST(Pose2, inverse ) EXPECT(assert_equal(l,lTg*g)); // Check derivative - Matrix numericalH = numericalDerivative11(testing::inverse, lTg, 1e-5); + Matrix numericalH = numericalDerivative11(testing::inverse, lTg); Matrix actualDinverse; lTg.inverse(actualDinverse); EXPECT(assert_equal(numericalH,actualDinverse)); @@ -415,7 +415,7 @@ TEST( Pose2, between ) 1.0, 0.0,-2.0, 0.0, 0.0,-1.0 ); - Matrix numericalH1 = numericalDerivative21(testing::between, gT1, gT2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::between, gT1, gT2); EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(numericalH1,actualH1)); // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx @@ -426,7 +426,7 @@ TEST( Pose2, between ) 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ); - Matrix numericalH2 = numericalDerivative22(testing::between, gT1, gT2, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::between, gT1, gT2); EXPECT(assert_equal(expectedH2,actualH2)); EXPECT(assert_equal(numericalH2,actualH2)); @@ -441,9 +441,9 @@ TEST( Pose2, between2 ) Matrix actualH1,actualH2; p1.between(p2,actualH1,actualH2); - Matrix numericalH1 = numericalDerivative21(testing::between, p1, p2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::between, p1, p2); EXPECT(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(testing::between, p1, p2, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::between, p1, p2); EXPECT(assert_equal(numericalH2,actualH2)); } @@ -488,9 +488,9 @@ TEST( Pose2, bearing ) EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual23)); // Check numerical derivatives - expectedH1 = numericalDerivative21(bearing_proxy, x2, l3, 1e-5); + expectedH1 = numericalDerivative21(bearing_proxy, x2, l3); EXPECT(assert_equal(expectedH1,actualH1)); - expectedH2 = numericalDerivative22(bearing_proxy, x2, l3, 1e-5); + expectedH2 = numericalDerivative22(bearing_proxy, x2, l3); EXPECT(assert_equal(expectedH1,actualH1)); // establish bearing is indeed 45 degrees even if rotated @@ -498,8 +498,8 @@ TEST( Pose2, bearing ) EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual34)); // Check numerical derivatives - expectedH1 = numericalDerivative21(bearing_proxy, x3, l4, 1e-5); - expectedH2 = numericalDerivative22(bearing_proxy, x3, l4, 1e-5); + expectedH1 = numericalDerivative21(bearing_proxy, x3, l4); + expectedH2 = numericalDerivative22(bearing_proxy, x3, l4); EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1)); } @@ -523,8 +523,8 @@ TEST( Pose2, range ) EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); // Check numerical derivatives - expectedH1 = numericalDerivative21(range_proxy, x2, l3, 1e-5); - expectedH2 = numericalDerivative22(range_proxy, x2, l3, 1e-5); + expectedH1 = numericalDerivative21(range_proxy, x2, l3); + expectedH2 = numericalDerivative22(range_proxy, x2, l3); EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH2,actualH2)); @@ -533,8 +533,8 @@ TEST( Pose2, range ) EXPECT_DOUBLES_EQUAL(2,actual34,1e-9); // Check numerical derivatives - expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5); - expectedH2 = numericalDerivative22(range_proxy, x3, l4, 1e-5); + expectedH1 = numericalDerivative21(range_proxy, x3, l4); + expectedH2 = numericalDerivative22(range_proxy, x3, l4); EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH2,actualH2)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 1065b35c9..a5788ce38 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -275,10 +275,10 @@ TEST( Pose3, compose ) Matrix actualDcompose1, actualDcompose2; T2.compose(T2, actualDcompose1, actualDcompose2); - Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2, 1e-2); + Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2); CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); - Matrix numericalH2 = numericalDerivative22(testing::compose, T2, T2, 1e-2); + Matrix numericalH2 = numericalDerivative22(testing::compose, T2, T2); CHECK(assert_equal(numericalH2,actualDcompose2)); } @@ -293,10 +293,10 @@ TEST( Pose3, compose2 ) Matrix actualDcompose1, actualDcompose2; T1.compose(T2, actualDcompose1, actualDcompose2); - Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2, 1e-2); + Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2); CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); - Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2, 1e-2); + Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2); CHECK(assert_equal(numericalH2,actualDcompose2)); } @@ -308,7 +308,7 @@ TEST( Pose3, inverse) Matrix expected = inverse(T.matrix()); CHECK(assert_equal(actual,expected,1e-8)); - Matrix numericalH = numericalDerivative11(testing::inverse, T, 1e-2); + Matrix numericalH = numericalDerivative11(testing::inverse, T); CHECK(assert_equal(numericalH,actualDinverse,5e-3)); } @@ -319,7 +319,7 @@ TEST( Pose3, inverseDerivatives2) Point3 t(3.5,-8.2,4.2); Pose3 T(R,t); - Matrix numericalH = numericalDerivative11(testing::inverse, T, 1e-2); + Matrix numericalH = numericalDerivative11(testing::inverse, T); Matrix actualDinverse; T.inverse(actualDinverse); CHECK(assert_equal(numericalH,actualDinverse,5e-3)); @@ -564,10 +564,10 @@ TEST( Pose3, between ) Pose3 actual = T2.between(T3, actualDBetween1,actualDBetween2); CHECK(assert_equal(expected,actual)); - Matrix numericalH1 = numericalDerivative21(testing::between , T2, T3, 1e-2); + Matrix numericalH1 = numericalDerivative21(testing::between , T2, T3); CHECK(assert_equal(numericalH1,actualDBetween1,5e-3)); - Matrix numericalH2 = numericalDerivative22(testing::between , T2, T3, 1e-2); + Matrix numericalH2 = numericalDerivative22(testing::between , T2, T3); CHECK(assert_equal(numericalH2,actualDBetween2)); } @@ -595,8 +595,8 @@ TEST( Pose3, range ) EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); // Check numerical derivatives - expectedH1 = numericalDerivative21(range_proxy, x2, l3, 1e-5); - expectedH2 = numericalDerivative22(range_proxy, x2, l3, 1e-5); + expectedH1 = numericalDerivative21(range_proxy, x2, l3); + expectedH2 = numericalDerivative22(range_proxy, x2, l3); EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH2,actualH2)); @@ -605,8 +605,8 @@ TEST( Pose3, range ) EXPECT_DOUBLES_EQUAL(5,actual34,1e-9); // Check numerical derivatives - expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5); - expectedH2 = numericalDerivative22(range_proxy, x3, l4, 1e-5); + expectedH1 = numericalDerivative21(range_proxy, x3, l4); + expectedH2 = numericalDerivative22(range_proxy, x3, l4); EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH2,actualH2)); } diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 512afb620..6df2939d1 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -111,7 +111,7 @@ TEST( Rot2, relativeBearing ) CHECK(assert_equal(Rot2(),actual1)); // Check numerical derivative - expectedH = numericalDerivative11(relativeBearing_, l1, 1e-5); + expectedH = numericalDerivative11(relativeBearing_, l1); CHECK(assert_equal(expectedH,actualH)); // establish relativeBearing is indeed 45 degrees @@ -119,7 +119,7 @@ TEST( Rot2, relativeBearing ) CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2)); // Check numerical derivative - expectedH = numericalDerivative11(relativeBearing_, l2, 1e-5); + expectedH = numericalDerivative11(relativeBearing_, l2); CHECK(assert_equal(expectedH,actualH)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index ab51034a6..d56a9308e 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -294,7 +294,7 @@ TEST( Rot3, inverse ) CHECK(assert_equal(I,R*R.inverse(actualH))); CHECK(assert_equal(I,R.inverse()*R)); - Matrix numericalH = numericalDerivative11(testing::inverse, R, 1e-2); + Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); } @@ -314,10 +314,10 @@ TEST( Rot3, between ) Rot3 actual = R1.between(R2, actualH1, actualH2); CHECK(assert_equal(expected,actual)); - Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2, 1e-2); + Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); CHECK(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2, 1e-2); + Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); CHECK(assert_equal(numericalH2,actualH2)); } diff --git a/gtsam/slam/tests/testPose2Factor.cpp b/gtsam/slam/tests/testPose2Factor.cpp index 0fd161350..87a7335a0 100644 --- a/gtsam/slam/tests/testPose2Factor.cpp +++ b/gtsam/slam/tests/testPose2Factor.cpp @@ -138,9 +138,9 @@ TEST( Pose2Factor, linearize ) CHECK(assert_equal(expected,*actual)); // Numerical do not work out because BetweenFactor is approximate ? - Matrix numericalH1 = numericalDerivative21(h, p1, p2, 1e-5); + Matrix numericalH1 = numericalDerivative21(h, p1, p2); CHECK(assert_equal(expectedH1,numericalH1)); - Matrix numericalH2 = numericalDerivative22(h, p1, p2, 1e-5); + Matrix numericalH2 = numericalDerivative22(h, p1, p2); CHECK(assert_equal(expectedH2,numericalH2)); } diff --git a/gtsam/slam/tests/testPose2Prior.cpp b/gtsam/slam/tests/testPose2Prior.cpp index fff6208d2..b83ac0681 100644 --- a/gtsam/slam/tests/testPose2Prior.cpp +++ b/gtsam/slam/tests/testPose2Prior.cpp @@ -89,7 +89,7 @@ TEST( Pose2Prior, linearize ) boost::shared_ptr actual = factor.linearize(x0, ordering); // Test with numerical derivative - Matrix numericalH = numericalDerivative11(h, prior, 1e-5); + Matrix numericalH = numericalDerivative11(h, prior); CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering["x1"])))); }