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

Fix round-off errors that fail testRot3 and testPose3 in Linux-32bit. 

For delta=1e-5, numericalDerivative2x(...) returns Jacobian with all 0 entries.
Use delta=1e-4!

testTrifocal still fails with sign inversion problem. Don't know why.
release/4.3a0
Duy-Nguyen Ta 2010-10-21 04:41:42 +00:00
parent c4df80df55
commit 7307a5c8d7
4 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
geometry/Rot3.cpp
View File

@ -199,7 +199,7 @@ namespace gtsam {
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return *this * R2;
}

2
geometry/Rot3.h
View File

@ -160,7 +160,7 @@ namespace gtsam {
/** Exponential map at identity - create a rotation from canonical coordinates
* using Rodriguez' formula
*/
static Rot3 Expmap(const Vector& v) {
static Rot3 Expmap(const Vector& v) {
if(zero(v)) return Rot3();
else return rodriguez(v);
}

12
geometry/tests/testPose3.cpp
View File

@ -168,10 +168,10 @@ TEST( Pose3, compose )
Matrix actualDcompose1, actualDcompose2;
T2.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2, 1e-5);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2, 1e-4);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2, 1e-5);
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2, 1e-4);
CHECK(assert_equal(numericalH2,actualDcompose2));
}
@ -186,10 +186,10 @@ TEST( Pose3, compose2 )
Matrix actualDcompose1, actualDcompose2;
T1.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2, 1e-5);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2, 1e-4);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2, 1e-5);
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2, 1e-4);
CHECK(assert_equal(numericalH2,actualDcompose2));
}
@ -457,10 +457,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-5);
Matrix numericalH1 = numericalDerivative21(testing::between<Pose3> , T2, T3, 1e-4);
CHECK(assert_equal(numericalH1,actualDBetween1,5e-5));
Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3, 1e-5);
Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3, 1e-4);
CHECK(assert_equal(numericalH2,actualDBetween2));
}

10
geometry/tests/testRot3.cpp
View File

@ -276,11 +276,11 @@ TEST( Rot3, compose )
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3>, R1,
R2, 1e-5);
R2, 1e-4);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3>, R1,
R2, 1e-5);
R2, 1e-4);
CHECK(assert_equal(numericalH2,actualH2));
}
@ -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-5);
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R, 1e-4);
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-5);
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2, 1e-4);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2, 1e-5);
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2, 1e-4);
CHECK(assert_equal(numericalH2,actualH2));
}