12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

moving to squared sampson error

release/4.3a0
Ayush Baid 2021-06-09 16:18:14 -07:00
parent e9738c70a6
commit 65211f8b0a
4 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/EssentialMatrix.cpp
View File

@ -154,25 +154,27 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB,
Matrix15 numerator_H; Matrix15 numerator_H;
numerator_H << numerator_H_R, numerator_H_D; numerator_H << numerator_H_R, numerator_H_D;
*HE = numerator_H / denominator - *HE = 2 * sampson_error * (numerator_H / denominator -
algebraic_error * denominator_H / (denominator * denominator); algebraic_error * denominator_H / (denominator * denominator));
} }
if (HvA){ if (HvA){
Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose(); Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose();
Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / denominator; 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){ if (HvB){
Matrix13 numerator_H_vB = vA.transpose() * matrix(); Matrix13 numerator_H_vB = vA.transpose() * matrix();
Matrix13 denominator_H_vB = nB.transpose() * I * matrix() / denominator; 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;
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/geometry/EssentialMatrix.h
View File

@ -159,7 +159,7 @@ class EssentialMatrix {
return E.rotate(cRb); return E.rotate(cRb);
} }
/// epipolar error, sampson /// epipolar error, sampson squared
GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB,
OptionalJacobian<1, 5> HE = boost::none, OptionalJacobian<1, 5> HE = boost::none,
OptionalJacobian<1, 3> HvA = boost::none, OptionalJacobian<1, 3> HvA = boost::none,

4
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -254,8 +254,8 @@ TEST(EssentialMatrix, errorValue) {
// algebraic error = 5 // algebraic error = 5
// norm of line for b = 1 // norm of line for b = 1
// norm of line for a = 1 // norm of line for a = 1
// sampson error = 5 / sqrt(1^2 + 1^2) // sampson error = 5^2 / 1^2 + 1^2
double expected = 3.535533906; double expected = 12.5;
// check the error // check the error
double actual = trueE.error(a, b); double actual = trueE.error(a, b);

8
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -24,7 +24,7 @@ using namespace gtsam;
// Noise model for first type of factor is evaluating algebraic error // Noise model for first type of factor is evaluating algebraic error
noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1,
0.01); 1e-4);
// Noise model for second type of factor is evaluating pixel coordinates // Noise model for second type of factor is evaluating pixel coordinates
noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2);
@ -196,9 +196,9 @@ TEST (EssentialMatrixFactor, minimization) {
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE); initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) #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 #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 #endif
// Optimize // Optimize
@ -410,7 +410,7 @@ TEST (EssentialMatrixFactor, extraMinimization) {
initial.insert(1, initialE); initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) #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 #else
EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
#endif #endif