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

Merge branch 'feature/sampson-epipolar-error' into feature/essential-mat-with-approx-k

release/4.3a0
Ayush Baid 2021-06-09 16:31:05 -07:00
parent 15f8b416cd 65211f8b0a
commit 2e69d09732
4 changed files with 23 additions and 17 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;
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;
}
/* ************************************************************************* */

2
gtsam/geometry/EssentialMatrix.h
View File

@ -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,

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

@ -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);

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

@ -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<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> 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<EssentialMatrix> E_(key); // leaf expression
Expression<double> 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<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> f =
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
boost::function<EssentialMatrix(const Rot3&, const Unit3&, OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)> g;
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none,
boost::none);
boost::function<EssentialMatrix(const Rot3&, const Unit3&,
OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)>
g;
Expression<Rot3> R_(key);
Expression<Unit3> d_(trueDirection);
Expression<EssentialMatrix> 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