diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 2bc4c58f0..89475eba6 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -130,15 +130,20 @@ TEST( Rot3, rodriguez4) Rot3 expected(c,-s, 0, s, c, 0, 0, 0, 1); - CHECK(assert_equal(expected,actual,1e-5)); - CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5)); + CHECK(assert_equal(expected,actual)); + CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual)); } /* ************************************************************************* */ TEST( Rot3, retract) { Vector v = zero(3); - CHECK(assert_equal(R.retract(v), R)); + CHECK(assert_equal(R, R.retract(v))); + + // test Canonical coordinates + Canonical chart; + Vector v2 = chart.apply(R); + CHECK(assert_equal(R, chart.retract(v2))); } /* ************************************************************************* */ @@ -201,9 +206,9 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ TEST( Rot3, rightJacobianExpMapSO3 ) { // Linearization point - Vector thetahat = (Vector(3) << 0.1, 0, 0); + Vector3 thetahat; thetahat << 0.1, 0, 0; - Matrix expectedJacobian = numericalDerivative11( + Matrix expectedJacobian = numericalDerivative11( boost::bind(&Rot3::Expmap, _1), thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); CHECK(assert_equal(expectedJacobian, actualJacobian)); @@ -213,10 +218,10 @@ TEST( Rot3, rightJacobianExpMapSO3 ) TEST( Rot3, rightJacobianExpMapSO3inverse ) { // Linearization point - Vector thetahat = (Vector(3) << 0.1,0.1,0); ///< Current estimate of rotation rate bias - Vector deltatheta = (Vector(3) << 0, 0, 0); + Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + Vector3 deltatheta; deltatheta << 0, 0, 0; - Matrix expectedJacobian = numericalDerivative11( + Matrix expectedJacobian = numericalDerivative11( boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); EXPECT(assert_equal(expectedJacobian, actualJacobian)); @@ -373,10 +378,10 @@ TEST( Rot3, between ) EXPECT(assert_equal(expected,actual)); Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); - CHECK(assert_equal(numericalH1,actualH1, 1e-4)); + CHECK(assert_equal(numericalH1,actualH1)); Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); - CHECK(assert_equal(numericalH2,actualH2, 1e-4)); + CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ @@ -392,12 +397,12 @@ Vector3 testDexpL(const Vector3& dw) { TEST( Rot3, dexpL) { Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, - LieVector(zero(3)), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); Matrix actualDexpInvL = Rot3::dexpInvL(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); } /* ************************************************************************* */ @@ -510,7 +515,7 @@ TEST( Rot3, logmapStability ) { // std::cout << "trace: " << tr << std::endl; // R.print("R = "); Vector actualw = Rot3::Logmap(R); - CHECK(assert_equal(w, actualw, 1e-15)); // this should be fixed for Quaternions!!! + CHECK(assert_equal(w, actualw, 1e-15)); } /* ************************************************************************* */