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"])))); }