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

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.

release/4.3a0
Richard Roberts 2011-04-08 18:41:50 +00:00
parent b1922464de
commit 1a94808f57
10 changed files with 55 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Pose2.cpp
View File

@ -51,7 +51,7 @@ namespace gtsam {
assert(xi.size() == 3); assert(xi.size() == 3);
Point2 v(xi(0),xi(1)); Point2 v(xi(0),xi(1));
double w = xi(2); double w = xi(2);
if (fabs(w) < 1e-5) if (fabs(w) < 1e-10)
return Pose2(xi[0], xi[1], xi[2]); return Pose2(xi[0], xi[1], xi[2]);
else { else {
Rot2 R(Rot2::fromAngle(w)); Rot2 R(Rot2::fromAngle(w));
@ -66,7 +66,7 @@ namespace gtsam {
const Rot2& R = p.r(); const Rot2& R = p.r();
const Point2& t = p.t(); const Point2& t = p.t();
double w = R.theta(); double w = R.theta();
if (fabs(w) < 1e-5) if (fabs(w) < 1e-10)
return Vector_(3, t.x(), t.y(), w); return Vector_(3, t.x(), t.y(), w);
else { else {
double c_1 = R.c()-1.0, s = R.s(); double c_1 = R.c()-1.0, s = R.s();
@ -216,7 +216,7 @@ namespace gtsam {
Point2 d = transform_to(point, H1, H2); Point2 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
Matrix D_result_d; Matrix D_result_d;
if(fabs(n) > 1e-5) if(fabs(n) > 1e-10)
D_result_d = Matrix_(1, 2, x / n, y / n); D_result_d = Matrix_(1, 2, x / n, y / n);
else { else {
D_result_d = Matrix_(1,2, 1.0, 1.0); D_result_d = Matrix_(1,2, 1.0, 1.0);

4
gtsam/geometry/Pose3.cpp
View File

@ -60,7 +60,7 @@ namespace gtsam {
Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5)); Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5));
double theta = w.norm(); double theta = w.norm();
if (theta < 1e-5) { if (theta < 1e-10) {
static const Rot3 I; static const Rot3 I;
return Pose3(I, v); return Pose3(I, v);
} }
@ -78,7 +78,7 @@ namespace gtsam {
Vector Pose3::LogmapFull(const Pose3& p) { Vector Pose3::LogmapFull(const Pose3& p) {
Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = norm_2(w); double t = norm_2(w);
if (t < 1e-5) if (t < 1e-10)
return concatVectors(2, &w, &T); return concatVectors(2, &w, &T);
else { else {
Matrix W = skewSymmetric(w/t); Matrix W = skewSymmetric(w/t);

2
gtsam/geometry/Rot2.cpp
View File

@ -55,7 +55,7 @@ bool Rot2::equals(const Rot2& R, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
Rot2& Rot2::normalize() { Rot2& Rot2::normalize() {
double scale = c_*c_ + s_*s_; 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); scale = pow(scale, -0.5);
c_ *= scale; c_ *= scale;
s_ *= scale; s_ *= scale;

12
gtsam/geometry/Rot3.cpp
View File

@ -105,7 +105,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) { Rot3 Rot3::rodriguez(const Vector& w) {
double t = norm_2(w); double t = norm_2(w);
if (t < 1e-5) return Rot3(); if (t < 1e-10) return Rot3();
return rodriguez(w/t, t); return rodriguez(w/t, t);
} }
@ -156,16 +156,16 @@ namespace gtsam {
// Log map at identity - return the canonical coordinates of this rotation // Log map at identity - return the canonical coordinates of this rotation
Vector Rot3::Logmap(const Rot3& R) { Vector Rot3::Logmap(const Rot3& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z(); 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); return zero(3);
} else if (fabs(tr - -1.0) < 1e-5) { // when theta = +-pi, +-3pi, +-5pi, etc. } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
if(R.r3().z() != -1.0) if(fabs(R.r3().z() - -1.0) > 1e-10)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) * return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
Vector_(3, R.r3().x(), R.r3().y(), 1.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<double>() / sqrt(2.0+2.0*R.r2().y())) * return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) *
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); 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<double>() / sqrt(2.0+2.0*R.r1().x())) * return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
} else { } else {

46
gtsam/geometry/tests/testPose2.cpp
View File

@ -204,11 +204,11 @@ TEST( Pose2, transform_to )
EXPECT(assert_equal(expected,actual)); EXPECT(assert_equal(expected,actual));
EXPECT(assert_equal(expectedH1,actualH1)); 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(numericalH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2)); 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)); 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 H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.); 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, H1));
EXPECT(assert_equal(H1_expected, numericalH1)); 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, H2));
EXPECT(assert_equal(H2_expected, numericalH2)); EXPECT(assert_equal(H2_expected, numericalH2));
} }
@ -258,8 +258,8 @@ TEST(Pose2, compose_a)
0.0, 0.0, 1.0 0.0, 0.0, 1.0
); );
Matrix expectedH2 = eye(3); Matrix expectedH2 = eye(3);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5); Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5); Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
EXPECT(assert_equal(expectedH1,actualDcompose1)); EXPECT(assert_equal(expectedH1,actualDcompose1));
EXPECT(assert_equal(numericalH1,actualDcompose1)); EXPECT(assert_equal(numericalH1,actualDcompose1));
EXPECT(assert_equal(expectedH2,actualDcompose2)); EXPECT(assert_equal(expectedH2,actualDcompose2));
@ -285,8 +285,8 @@ TEST(Pose2, compose_b)
Matrix actualDcompose1, actualDcompose2; Matrix actualDcompose1, actualDcompose2;
Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2); Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5); Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5); Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5)); EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
EXPECT(assert_equal(numericalH2,actualDcompose2)); EXPECT(assert_equal(numericalH2,actualDcompose2));
@ -306,8 +306,8 @@ TEST(Pose2, compose_c)
Matrix actualDcompose1, actualDcompose2; Matrix actualDcompose1, actualDcompose2;
Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2); Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5); Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5); Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5)); EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
EXPECT(assert_equal(numericalH2,actualDcompose2)); EXPECT(assert_equal(numericalH2,actualDcompose2));
@ -330,7 +330,7 @@ TEST(Pose2, inverse )
EXPECT(assert_equal(l,lTg*g)); EXPECT(assert_equal(l,lTg*g));
// Check derivative // Check derivative
Matrix numericalH = numericalDerivative11<Pose2,Pose2>(testing::inverse, lTg, 1e-5); Matrix numericalH = numericalDerivative11<Pose2,Pose2>(testing::inverse, lTg);
Matrix actualDinverse; Matrix actualDinverse;
lTg.inverse(actualDinverse); lTg.inverse(actualDinverse);
EXPECT(assert_equal(numericalH,actualDinverse)); EXPECT(assert_equal(numericalH,actualDinverse));
@ -415,7 +415,7 @@ TEST( Pose2, between )
1.0, 0.0,-2.0, 1.0, 0.0,-2.0,
0.0, 0.0,-1.0 0.0, 0.0,-1.0
); );
Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2, 1e-5); Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(numericalH1,actualH1)); EXPECT(assert_equal(numericalH1,actualH1));
// Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx // 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, 1.0, 0.0,
0.0, 0.0, 1.0 0.0, 0.0, 1.0
); );
Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2, 1e-5); Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
EXPECT(assert_equal(expectedH2,actualH2)); EXPECT(assert_equal(expectedH2,actualH2));
EXPECT(assert_equal(numericalH2,actualH2)); EXPECT(assert_equal(numericalH2,actualH2));
@ -441,9 +441,9 @@ TEST( Pose2, between2 )
Matrix actualH1,actualH2; Matrix actualH1,actualH2;
p1.between(p2,actualH1,actualH2); p1.between(p2,actualH1,actualH2);
Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2, 1e-5); Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2);
EXPECT(assert_equal(numericalH1,actualH1)); EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2, 1e-5); Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2);
EXPECT(assert_equal(numericalH2,actualH2)); EXPECT(assert_equal(numericalH2,actualH2));
} }
@ -488,9 +488,9 @@ TEST( Pose2, bearing )
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual23)); EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual23));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3, 1e-5); expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
expectedH2 = numericalDerivative22(bearing_proxy, x2, l3, 1e-5); expectedH2 = numericalDerivative22(bearing_proxy, x2, l3);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
// establish bearing is indeed 45 degrees even if rotated // 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)); EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual34));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4, 1e-5); expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
expectedH2 = numericalDerivative22(bearing_proxy, x3, l4, 1e-5); expectedH2 = numericalDerivative22(bearing_proxy, x3, l4);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
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); EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x2, l3, 1e-5); expectedH1 = numericalDerivative21(range_proxy, x2, l3);
expectedH2 = numericalDerivative22(range_proxy, x2, l3, 1e-5); expectedH2 = numericalDerivative22(range_proxy, x2, l3);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2)); EXPECT(assert_equal(expectedH2,actualH2));
@ -533,8 +533,8 @@ TEST( Pose2, range )
EXPECT_DOUBLES_EQUAL(2,actual34,1e-9); EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5); expectedH1 = numericalDerivative21(range_proxy, x3, l4);
expectedH2 = numericalDerivative22(range_proxy, x3, l4, 1e-5); expectedH2 = numericalDerivative22(range_proxy, x3, l4);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2)); EXPECT(assert_equal(expectedH2,actualH2));
} }

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

@ -275,10 +275,10 @@ TEST( Pose3, compose )
Matrix actualDcompose1, actualDcompose2; Matrix actualDcompose1, actualDcompose2;
T2.compose(T2, actualDcompose1, actualDcompose2); T2.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2, 1e-2); Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); CHECK(assert_equal(numericalH1,actualDcompose1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2, 1e-2); Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2);
CHECK(assert_equal(numericalH2,actualDcompose2)); CHECK(assert_equal(numericalH2,actualDcompose2));
} }
@ -293,10 +293,10 @@ TEST( Pose3, compose2 )
Matrix actualDcompose1, actualDcompose2; Matrix actualDcompose1, actualDcompose2;
T1.compose(T2, actualDcompose1, actualDcompose2); T1.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2, 1e-2); Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); CHECK(assert_equal(numericalH1,actualDcompose1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2, 1e-2); Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2);
CHECK(assert_equal(numericalH2,actualDcompose2)); CHECK(assert_equal(numericalH2,actualDcompose2));
} }
@ -308,7 +308,7 @@ TEST( Pose3, inverse)
Matrix expected = inverse(T.matrix()); Matrix expected = inverse(T.matrix());
CHECK(assert_equal(actual,expected,1e-8)); CHECK(assert_equal(actual,expected,1e-8));
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T, 1e-2); Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
CHECK(assert_equal(numericalH,actualDinverse,5e-3)); CHECK(assert_equal(numericalH,actualDinverse,5e-3));
} }
@ -319,7 +319,7 @@ TEST( Pose3, inverseDerivatives2)
Point3 t(3.5,-8.2,4.2); Point3 t(3.5,-8.2,4.2);
Pose3 T(R,t); Pose3 T(R,t);
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T, 1e-2); Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
Matrix actualDinverse; Matrix actualDinverse;
T.inverse(actualDinverse); T.inverse(actualDinverse);
CHECK(assert_equal(numericalH,actualDinverse,5e-3)); CHECK(assert_equal(numericalH,actualDinverse,5e-3));
@ -564,10 +564,10 @@ TEST( Pose3, between )
Pose3 actual = T2.between(T3, actualDBetween1,actualDBetween2); Pose3 actual = T2.between(T3, actualDBetween1,actualDBetween2);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::between<Pose3> , T2, T3, 1e-2); Matrix numericalH1 = numericalDerivative21(testing::between<Pose3> , T2, T3);
CHECK(assert_equal(numericalH1,actualDBetween1,5e-3)); CHECK(assert_equal(numericalH1,actualDBetween1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3, 1e-2); Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3);
CHECK(assert_equal(numericalH2,actualDBetween2)); CHECK(assert_equal(numericalH2,actualDBetween2));
} }
@ -595,8 +595,8 @@ TEST( Pose3, range )
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x2, l3, 1e-5); expectedH1 = numericalDerivative21(range_proxy, x2, l3);
expectedH2 = numericalDerivative22(range_proxy, x2, l3, 1e-5); expectedH2 = numericalDerivative22(range_proxy, x2, l3);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2)); EXPECT(assert_equal(expectedH2,actualH2));
@ -605,8 +605,8 @@ TEST( Pose3, range )
EXPECT_DOUBLES_EQUAL(5,actual34,1e-9); EXPECT_DOUBLES_EQUAL(5,actual34,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5); expectedH1 = numericalDerivative21(range_proxy, x3, l4);
expectedH2 = numericalDerivative22(range_proxy, x3, l4, 1e-5); expectedH2 = numericalDerivative22(range_proxy, x3, l4);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2)); EXPECT(assert_equal(expectedH2,actualH2));
} }

4
gtsam/geometry/tests/testRot2.cpp
View File

@ -111,7 +111,7 @@ TEST( Rot2, relativeBearing )
CHECK(assert_equal(Rot2(),actual1)); CHECK(assert_equal(Rot2(),actual1));
// Check numerical derivative // Check numerical derivative
expectedH = numericalDerivative11(relativeBearing_, l1, 1e-5); expectedH = numericalDerivative11(relativeBearing_, l1);
CHECK(assert_equal(expectedH,actualH)); CHECK(assert_equal(expectedH,actualH));
// establish relativeBearing is indeed 45 degrees // establish relativeBearing is indeed 45 degrees
@ -119,7 +119,7 @@ TEST( Rot2, relativeBearing )
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2)); CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2));
// Check numerical derivative // Check numerical derivative
expectedH = numericalDerivative11(relativeBearing_, l2, 1e-5); expectedH = numericalDerivative11(relativeBearing_, l2);
CHECK(assert_equal(expectedH,actualH)); CHECK(assert_equal(expectedH,actualH));
} }

6
gtsam/geometry/tests/testRot3.cpp
View File

@ -294,7 +294,7 @@ TEST( Rot3, inverse )
CHECK(assert_equal(I,R*R.inverse(actualH))); CHECK(assert_equal(I,R*R.inverse(actualH)));
CHECK(assert_equal(I,R.inverse()*R)); CHECK(assert_equal(I,R.inverse()*R));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R, 1e-2); Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
CHECK(assert_equal(numericalH,actualH)); CHECK(assert_equal(numericalH,actualH));
} }
@ -314,10 +314,10 @@ TEST( Rot3, between )
Rot3 actual = R1.between(R2, actualH1, actualH2); Rot3 actual = R1.between(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2, 1e-2); Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH1,actualH1)); CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2, 1e-2); Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2));
} }

4
gtsam/slam/tests/testPose2Factor.cpp
View File

@ -138,9 +138,9 @@ TEST( Pose2Factor, linearize )
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected,*actual));
// Numerical do not work out because BetweenFactor is approximate ? // 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)); CHECK(assert_equal(expectedH1,numericalH1));
Matrix numericalH2 = numericalDerivative22(h, p1, p2, 1e-5); Matrix numericalH2 = numericalDerivative22(h, p1, p2);
CHECK(assert_equal(expectedH2,numericalH2)); CHECK(assert_equal(expectedH2,numericalH2));
} }

2
gtsam/slam/tests/testPose2Prior.cpp
View File

@ -89,7 +89,7 @@ TEST( Pose2Prior, linearize )
boost::shared_ptr<JacobianFactor> actual = factor.linearize(x0, ordering); boost::shared_ptr<JacobianFactor> actual = factor.linearize(x0, ordering);
// Test with numerical derivative // 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"])))); CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering["x1"]))));
} }