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);
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);

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));
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);

2
gtsam/geometry/Rot2.cpp
View File

@ -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;

12
gtsam/geometry/Rot3.cpp
View File

@ -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<double>() / 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<double>() / 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<double>() / sqrt(2.0+2.0*R.r1().x())) *
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
} 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(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<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<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);
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<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<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);
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<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<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);
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<Pose2,Pose2>(testing::inverse, lTg, 1e-5);
Matrix numericalH = numericalDerivative11<Pose2,Pose2>(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<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(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<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(numericalH2,actualH2));
@ -441,9 +441,9 @@ TEST( Pose2, between2 )
Matrix 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));
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));
}
@ -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));
}

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

@ -275,10 +275,10 @@ TEST( Pose3, compose )
Matrix 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));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2, 1e-2);
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, 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<Pose3>, T1, T2, 1e-2);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2);
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));
}
@ -308,7 +308,7 @@ TEST( Pose3, inverse)
Matrix expected = inverse(T.matrix());
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));
}
@ -319,7 +319,7 @@ TEST( Pose3, inverseDerivatives2)
Point3 t(3.5,-8.2,4.2);
Pose3 T(R,t);
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T, 1e-2);
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, 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<Pose3> , T2, T3, 1e-2);
Matrix numericalH1 = numericalDerivative21(testing::between<Pose3> , T2, T3);
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));
}
@ -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));
}

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

@ -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));
}

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.inverse()*R));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R, 1e-2);
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, 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<Rot3> , R1, R2, 1e-2);
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
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));
}

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

@ -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));
}

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

@ -89,7 +89,7 @@ TEST( Pose2Prior, linearize )
boost::shared_ptr<JacobianFactor> 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"]))));
}