Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Switching to squared sampson point-line error #792

Merged
merged 5 commits into from
Jun 21, 2021
Merged

Switching to squared sampson point-line error #792

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
8a2ce7e
switching to sampson point line error
ayushbaid Jun 9, 2021
0ecd8ab
fixing jacobians and reformatting
ayushbaid Jun 9, 2021
ae69e5f
changing error values in test
ayushbaid Jun 9, 2021
e9738c7
adding jacobians on input points
ayushbaid Jun 9, 2021
65211f8
moving to squared sampson error
ayushbaid Jun 9, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
80 changes: 72 additions & 8 deletions gtsam/geometry/EssentialMatrix.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,16 +101,80 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
}

/* ************************************************************************* */
double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
OptionalJacobian<1, 5> H) const {
if (H) {
double EssentialMatrix::error(const Vector3& vA, const Vector3& vB,
OptionalJacobian<1, 5> HE,
OptionalJacobian<1, 3> HvA,
OptionalJacobian<1, 3> HvB) const {
// compute the epipolar lines
Point3 lB = E_ * vB;
Point3 lA = E_.transpose() * vA;

// 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();
Matrix21 nA = I * lA;
Matrix21 nB = I * lB;
double nA_sq_norm = nA.squaredNorm();
double nB_sq_norm = nB.squaredNorm();

// compute the normalizing denominator and finally the sampson error
double denominator = sqrt(nA_sq_norm + nB_sq_norm);
double sampson_error = algebraic_error / denominator;

if (HE) {
// See math.lyx
Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
* direction().basis();
*H << HR, HD;
// computing the derivatives of the numerator w.r.t. E
Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix12 numerator_H_D = vA.transpose() *
skewSymmetric(-rotation().matrix() * vB) *
direction().basis();

// computing the derivatives of the denominator w.r.t. E
Matrix12 denominator_H_nA = nA.transpose() / denominator;
Matrix12 denominator_H_nB = nB.transpose() / denominator;
Matrix13 denominator_H_lA = denominator_H_nA * I;
Matrix13 denominator_H_lB = denominator_H_nB * I;
Matrix33 lB_H_R = E_ * skewSymmetric(-vB);
Matrix32 lB_H_D =
skewSymmetric(-rotation().matrix() * vB) * direction().basis();
Matrix33 lA_H_R = skewSymmetric(E_.matrix().transpose() * vA);
Matrix32 lA_H_D =
rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis();

Matrix13 denominator_H_R =
denominator_H_lA * lA_H_R + denominator_H_lB * lB_H_R;
Matrix12 denominator_H_D =
denominator_H_lA * lA_H_D + denominator_H_lB * lB_H_D;

Matrix15 denominator_H;
denominator_H << denominator_H_R, denominator_H_D;
Matrix15 numerator_H;
numerator_H << numerator_H_R, numerator_H_D;

*HE = 2 * sampson_error * (numerator_H / denominator -
algebraic_error * denominator_H / (denominator * denominator));
}
return dot(vA, E_ * vB);

if (HvA){
Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose();
Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / 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 = 2 * sampson_error * (numerator_H_vB / denominator -
algebraic_error * denominator_H_vB / (denominator * denominator));
}

return sampson_error * sampson_error;
}

/* ************************************************************************* */
Expand Down
6 changes: 4 additions & 2 deletions gtsam/geometry/EssentialMatrix.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,9 +159,11 @@ class EssentialMatrix {
return E.rotate(cRb);
}

/// epipolar error, algebraic
/// epipolar error, sampson squared
GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB,
OptionalJacobian<1, 5> H = boost::none) const;
OptionalJacobian<1, 5> HE = boost::none,
OptionalJacobian<1, 3> HvA = boost::none,
OptionalJacobian<1, 3> HvB = boost::none) const;

/// @}

Expand Down
62 changes: 61 additions & 1 deletion gtsam/geometry/tests/testEssentialMatrix.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -241,10 +241,70 @@ TEST (EssentialMatrix, epipoles) {
EXPECT(assert_equal(e2, E.epipole_b()));
}

//*************************************************************************
TEST(EssentialMatrix, errorValue) {
// Use two points to get error
Point3 a(1, -2, 1);
Point3 b(3, 1, 1);

// compute the expected error
// E = [0, 0, 0; 0, 0, -1; 1, 0, 0]
// line for b = [0, -1, 3]
// line for a = [1, 0, 2]
// algebraic error = 5
// norm of line for b = 1
// norm of line for a = 1
// sampson error = 5^2 / 1^2 + 1^2
double expected = 12.5;

// check the error
double actual = trueE.error(a, b);
EXPECT(assert_equal(expected, actual, 1e-6));
}

//*************************************************************************
double error_(const Rot3& R, const Unit3& t, const Point3& a, const Point3& b) {
EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t);
return E.error(a, b);
}
TEST(EssentialMatrix, errorJacobians) {
// Use two points to get error
Point3 vA(1, -2, 1);
Point3 vB(3, 1, 1);

Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
Point3 c1Tc2(0.4, 0.5, 0.6);
EssentialMatrix E(c1Rc2, Unit3(c1Tc2));

// Use numerical derivatives to calculate the expected Jacobian
Matrix13 HRexpected;
Matrix12 HDexpected;
Matrix13 HvAexpected;
Matrix13 HvBexpected;
HRexpected = numericalDerivative41<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(),
E.direction(), vA, vB);
HDexpected = numericalDerivative42<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(),
E.direction(), vA, vB);
HvAexpected = numericalDerivative43<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(),
E.direction(), vA, vB);
HvBexpected = numericalDerivative44<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(),
E.direction(), vA, vB);
Matrix15 HEexpected;
HEexpected << HRexpected, HDexpected;

Matrix15 HEactual;
Matrix13 HvAactual, HvBactual;
E.error(vA, vB, HEactual, HvAactual, HvBactual);

// Verify the Jacobian is correct
EXPECT(assert_equal(HEexpected, HEactual, 1e-5));
EXPECT(assert_equal(HvAexpected, HvAactual, 1e-5));
EXPECT(assert_equal(HvBexpected, HvBactual, 1e-5));
}

/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

20 changes: 12 additions & 8 deletions gtsam/slam/tests/testEssentialMatrixFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,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);

Expand Down Expand Up @@ -117,7 +117,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

Expand All @@ -143,9 +144,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_);
Expand Down Expand Up @@ -192,9 +196,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(643.26, 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
Expand Down Expand Up @@ -406,7 +410,7 @@ TEST (EssentialMatrixFactor, extraMinimization) {
initial.insert(1, initialE);

#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, 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
Expand Down