diff --git a/geometry/Rot3.cpp b/geometry/Rot3.cpp index e8cb6e8e6..b34fce069 100644 --- a/geometry/Rot3.cpp +++ b/geometry/Rot3.cpp @@ -199,7 +199,7 @@ namespace gtsam { /* ************************************************************************* */ Rot3 Rot3::compose (const Rot3& R2, boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); + if (H1) *H1 = R2.transpose(); if (H2) *H2 = I3; return *this * R2; } diff --git a/geometry/Rot3.h b/geometry/Rot3.h index 95b9a2635..45e03b37e 100644 --- a/geometry/Rot3.h +++ b/geometry/Rot3.h @@ -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); } diff --git a/geometry/tests/testPose3.cpp b/geometry/tests/testPose3.cpp index e80514598..8bcd0dc5e 100644 --- a/geometry/tests/testPose3.cpp +++ b/geometry/tests/testPose3.cpp @@ -168,10 +168,10 @@ TEST( Pose3, compose ) Matrix actualDcompose1, actualDcompose2; T2.compose(T2, actualDcompose1, actualDcompose2); - Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2, 1e-4); CHECK(assert_equal(numericalH1,actualDcompose1,5e-5)); - Matrix numericalH2 = numericalDerivative22(testing::compose, T2, T2, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::compose, 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, T1, T2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2, 1e-4); CHECK(assert_equal(numericalH1,actualDcompose1,5e-5)); - Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::compose, 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 , T2, T3, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::between , T2, T3, 1e-4); CHECK(assert_equal(numericalH1,actualDBetween1,5e-5)); - Matrix numericalH2 = numericalDerivative22(testing::between , T2, T3, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::between , T2, T3, 1e-4); CHECK(assert_equal(numericalH2,actualDBetween2)); } diff --git a/geometry/tests/testRot3.cpp b/geometry/tests/testRot3.cpp index ecadc5c68..3472dc49a 100644 --- a/geometry/tests/testRot3.cpp +++ b/geometry/tests/testRot3.cpp @@ -276,11 +276,11 @@ TEST( Rot3, compose ) CHECK(assert_equal(expected,actual)); Matrix numericalH1 = numericalDerivative21(testing::compose, R1, - R2, 1e-5); + R2, 1e-4); CHECK(assert_equal(numericalH1,actualH1)); Matrix numericalH2 = numericalDerivative22(testing::compose, 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, R, 1e-5); + Matrix numericalH = numericalDerivative11(testing::inverse, 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 , R1, R2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2, 1e-4); CHECK(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2, 1e-4); CHECK(assert_equal(numericalH2,actualH2)); }