diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index cfe76ff04..cf8f2a4b8 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -111,7 +111,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // compute the algebraic error as a simple dot product. double algebraic_error = dot(vA, lB); - + // compute the line-norms for the two lines Matrix23 I; I.setIdentity(); @@ -154,25 +154,27 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, Matrix15 numerator_H; numerator_H << numerator_H_R, numerator_H_D; - *HE = numerator_H / denominator - - algebraic_error * denominator_H / (denominator * denominator); + *HE = 2 * sampson_error * (numerator_H / denominator - + algebraic_error * denominator_H / (denominator * denominator)); } if (HvA){ Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose(); Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / denominator; - *HvA = numerator_H_vA / denominator - algebraic_error * denominator_H_vA / (denominator * denominator); + *HvA = 2 * sampson_error * (numerator_H_vA / denominator - + algebraic_error * denominator_H_vA / (denominator * denominator)); } if (HvB){ Matrix13 numerator_H_vB = vA.transpose() * matrix(); Matrix13 denominator_H_vB = nB.transpose() * I * matrix() / denominator; - *HvB = numerator_H_vB / denominator - algebraic_error * denominator_H_vB / (denominator * denominator); + *HvB = 2 * sampson_error * (numerator_H_vB / denominator - + algebraic_error * denominator_H_vB / (denominator * denominator)); } - return sampson_error; + return sampson_error * sampson_error; } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 0f8403bc8..5293f1c7f 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -159,7 +159,7 @@ class EssentialMatrix { return E.rotate(cRb); } - /// epipolar error, sampson + /// epipolar error, sampson squared GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, OptionalJacobian<1, 5> HE = boost::none, OptionalJacobian<1, 3> HvA = boost::none, diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index f079e15c5..999bf34b9 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -254,8 +254,8 @@ TEST(EssentialMatrix, errorValue) { // algebraic error = 5 // norm of line for b = 1 // norm of line for a = 1 - // sampson error = 5 / sqrt(1^2 + 1^2) - double expected = 3.535533906; + // sampson error = 5^2 / 1^2 + 1^2 + double expected = 12.5; // check the error double actual = trueE.error(a, b); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 78f4644c1..4c6c950da 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -25,7 +25,7 @@ using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 0.01); + 1e-4); // Noise model for second type of factor is evaluating pixel coordinates noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); @@ -120,7 +120,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, + boost::none); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -146,9 +147,12 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function, - OptionalJacobian<5, 2>)> g; + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, + boost::none); + boost::function, + OptionalJacobian<5, 2>)> + g; Expression R_(key); Expression d_(trueDirection); Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); @@ -195,9 +199,9 @@ 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(419.07, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(59403.51, graph.error(initial), 1e-2); #else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); # TODO: redo this error #endif // Optimize @@ -519,7 +523,7 @@ TEST(EssentialMatrixFactor, extraMinimization) { initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(313.85, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(59403.51, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif