diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index b3ff907d9..9ad30bc41 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -125,7 +125,7 @@ TEST (EssentialMatrix, rotate) { } // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); - EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH1, actH1, 1e-7)); // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8)); } @@ -149,7 +149,7 @@ TEST (EssentialMatrix, FromPose3_b) { EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } //************************************************************************* diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index fac2164d8..d0dbe7908 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -35,13 +35,13 @@ TEST(BetweenFactor, Rot3) { boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); - EXPECT(assert_equal(numericalH1,actualH1)); + EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); - EXPECT(assert_equal(numericalH2,actualH2)); + EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 8fcb5b6e5..5184393ac 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -63,8 +63,8 @@ TEST( EssentialMatrixConstraint, test ) { factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); + CHECK(assert_equal(expectedH2, actualH2, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 0766dbf8f..1e5674599 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -128,7 +128,11 @@ TEST (EssentialMatrixFactor, minimization) { EssentialMatrix initialE = trueE.retract( (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); initial.insert(1, initialE); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +#endif // Optimize LevenbergMarquardtParams parameters; @@ -339,7 +343,12 @@ TEST (EssentialMatrixFactor, extraMinimization) { EssentialMatrix initialE = trueE.retract( (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +#endif // Optimize LevenbergMarquardtParams parameters; diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index ca924aaae..f36405318 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -59,7 +59,11 @@ TEST (RotateFactor, test) { EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + Vector expectedE = (Vector(3) << -0.0248752, 0.202981, -0.0890529); +#else Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867); +#endif EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); Matrix actual, expected; @@ -97,7 +101,12 @@ TEST (RotateFactor, minimization) { double degree = M_PI / 180; Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1); +#else EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1); +#endif // Optimize LevenbergMarquardtParams parameters; @@ -120,7 +129,13 @@ TEST (RotateDirectionsFactor, test) { EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + Vector expectedE = (Vector(2) << -0.0890529, -0.202981); +#else Vector expectedE = (Vector(2) << -0.08867, -0.20197); +#endif + EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); Matrix actual, expected; @@ -160,7 +175,12 @@ TEST (RotateDirectionsFactor, minimization) { double degree = M_PI / 180; Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(3335.9, graph.error(initial), 1); +#else EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1); +#endif // Optimize LevenbergMarquardtParams parameters; diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 2fc471eab..bf9dc0e8e 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -95,12 +95,21 @@ TEST( PoseBetweenFactor, Error ) { // The expected error Vector expectedError(6); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + expectedError << -0.0298135267953815, + 0.0131341515747393, + 0.0968868439682154, + -0.145701634472172, + -0.134898525569125, + -0.0421026389164264; +#else expectedError << -0.029839512616488, 0.013145599455949, 0.096971291682578, -0.139187549519821, -0.142346243063553, -0.039088532100977; +#endif // Create a factor and calculate the error Key poseKey1(1); @@ -123,12 +132,23 @@ TEST( PoseBetweenFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << 0.0173358202010741, + 0.0222210698409755, + -0.0125032003886145, + 0.0263800787416566, + 0.00540285006310398, + 0.000175859555693563; +#else expectedError << 0.017337193670445, 0.022222830355243, -0.012504190982804, 0.026413288603739, 0.005237695303536, -0.000071612703633; +#endif + // Create a factor and calculate the error Key poseKey1(1); @@ -165,8 +185,8 @@ TEST( PoseBetweenFactor, Jacobian ) { factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); + CHECK(assert_equal(expectedH2, actualH2, 1e-6)); } /* ************************************************************************* */ @@ -194,8 +214,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) { Vector error = factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-6)); + CHECK(assert_equal(expectedH2, actualH2, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index 27bcd55ce..cbfa45431 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -90,12 +90,23 @@ TEST( PosePriorFactor, Error ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << -0.182948257976108, + 0.13851858011118, + -0.157375974517456, + 0.766913166076379, + -1.22976117053126, + 0.949345561430261; +#else expectedError << -0.184137861505414, 0.139419283914526, -0.158399296722242, 0.740211733817804, -1.198210282560946, 1.008156093015192; +#endif + // Create a factor and calculate the error Key poseKey(1); @@ -116,12 +127,22 @@ TEST( PosePriorFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << -0.0224998729281528, + 0.191947887288328, + 0.273826035236257, + 1.36483391560855, + -0.754590051075035, + 0.585710674473659; +#else expectedError << -0.022712885347328, 0.193765110165872, 0.276418420819283, 1.497519863757366, -0.549375791422721, 0.452761203187666; +#endif // Create a factor and calculate the error Key poseKey(1); @@ -152,7 +173,7 @@ TEST( PosePriorFactor, Jacobian ) { factor.evaluateError(pose, actualH1); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */ @@ -176,7 +197,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) { factor.evaluateError(pose, actualH1); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */