diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 4c6c950da..42b5c4919 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -199,7 +199,7 @@ TEST (EssentialMatrixFactor, minimization) { (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(59403.51, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(18161.79, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); # TODO: redo this error #endif @@ -361,7 +361,7 @@ TEST (EssentialMatrixFactor3, minimization) { //************************************************************************* TEST(EssentialMatrixFactor4, factor) { Key keyE(1); - Key keyK(1); + Key keyK(2); for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor4 factor(keyE, keyK, pA(i), pB(i), model1); @@ -408,7 +408,7 @@ TEST(EssentialMatrixFactor4, evaluateErrorJacobians) { Point2 pB(12.0, 15.0); EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); - EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5); } //************************************************************************* @@ -434,16 +434,16 @@ TEST(EssentialMatrixFactor4, minimization) { initial.insert(1, initialE); initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(2914.92, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: update this value too #endif // add prior factor for calibration - Vector5 priorNoiseModelSigma; - priorNoiseModelSigma << 0.1, 0.1, 0.01, 0.1, 0.1; - graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + // Vector5 priorNoiseModelSigma; + // priorNoiseModelSigma << 0.1, 0.1, 0.01, 0.1, 0.1; + // graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); // Optimize LevenbergMarquardtParams parameters; @@ -667,7 +667,7 @@ TEST(EssentialMatrixFactor4, extraMinimization) { initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.62, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(59418.96, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this #endif