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

fixing jacobians and reformatting

release/4.3a0
Ayush Baid 2021-06-09 09:49:32 -07:00
parent 8a2ce7e118
commit 0ecd8ab63d
2 changed files with 27 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/geometry/EssentialMatrix.cpp
View File

@ -101,17 +101,15 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
}
/* ************************************************************************* */
double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
OptionalJacobian<1, 5> H) const {
double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
OptionalJacobian<1, 5> H) 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);
double algebraic_error = dot(vA, lB);
// compute the line-norms for the two lines
Matrix23 I;
I.setIdentity();
@ -128,9 +126,9 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
// See math.lyx
// 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();
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;
@ -138,20 +136,24 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
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) *
rotation().matrix().transpose();
Matrix32 lA_H_D = rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis();
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;
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;
*H = numerator_H / denominator - algebraic_error * denominator_H / (denominator * denominator);
*H = numerator_H / denominator -
algebraic_error * denominator_H / (denominator * denominator);
}
return sampson_error;
}

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

@ -242,7 +242,7 @@ TEST (EssentialMatrix, epipoles) {
}
//*************************************************************************
TEST (EssentialMatrix, errorValue) {
TEST(EssentialMatrix, errorValue) {
// Use two points to get error
Point3 a(1, -2, 1);
Point3 b(3, 1, 1);
@ -254,7 +254,7 @@ 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)
// sampson error = 5 / sqrt(1^2 + 1^2)
double expected = 3.535533906;
// check the error
@ -263,7 +263,7 @@ TEST (EssentialMatrix, errorValue) {
}
//*************************************************************************
double error_(const Rot3& R, const Unit3& t){
double error_(const Rot3& R, const Unit3& t) {
// Use two points to get error
Point3 a(1, -2, 1);
Point3 b(3, 1, 1);
@ -271,7 +271,7 @@ double error_(const Rot3& R, const Unit3& t){
EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t);
return E.error(a, b);
}
TEST (EssentialMatrix, errorJacobians) {
TEST(EssentialMatrix, errorJacobians) {
// Use two points to get error
Point3 a(1, -2, 1);
Point3 b(3, 1, 1);
@ -283,10 +283,10 @@ TEST (EssentialMatrix, errorJacobians) {
// Use numerical derivatives to calculate the expected Jacobian
Matrix13 HRexpected;
Matrix12 HDexpected;
HRexpected = numericalDerivative21<double, Rot3, Unit3>(
error_, E.rotation(), E.direction(), 1e-8);
HDexpected = numericalDerivative22<double, Rot3, Unit3>(
error_, E.rotation(), E.direction(), 1e-8);
HRexpected = numericalDerivative21<double, Rot3, Unit3>(error_, E.rotation(),
E.direction());
HDexpected = numericalDerivative22<double, Rot3, Unit3>(error_, E.rotation(),
E.direction());
Matrix15 HEexpected;
HEexpected << HRexpected, HDexpected;
@ -294,7 +294,7 @@ TEST (EssentialMatrix, errorJacobians) {
E.error(a, b, HEactual);
// Verify the Jacobian is correct
EXPECT(assert_equal(HEexpected, HEactual, 1e-8));
EXPECT(assert_equal(HEexpected, HEactual, 1e-5));
}
/* ************************************************************************* */
@ -303,4 +303,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */