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

adding jacobians on input points

release/4.3a0
Ayush Baid 2021-06-09 15:22:08 -07:00
parent 582afe1dbb
commit 15f8b416cd
3 changed files with 41 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/geometry/EssentialMatrix.cpp
View File

@ -101,8 +101,10 @@ 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> HE,
OptionalJacobian<1, 3> HvA,
OptionalJacobian<1, 3> HvB) const {
// compute the epipolar lines
Point3 lB = E_ * vB;
Point3 lA = E_.transpose() * vA;
@ -122,7 +124,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
double denominator = sqrt(nA_sq_norm + nB_sq_norm);
double sampson_error = algebraic_error / denominator;
if (H) {
if (HE) {
// See math.lyx
// computing the derivatives of the numerator w.r.t. E
Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB);
@ -152,9 +154,24 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
Matrix15 numerator_H;
numerator_H << numerator_H_R, numerator_H_D;
*H = numerator_H / denominator -
*HE = 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);
}
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);
}
return sampson_error;
}

4
gtsam/geometry/EssentialMatrix.h
View File

@ -161,7 +161,9 @@ class EssentialMatrix {
/// epipolar error, sampson
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;
/// @}

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

@ -263,18 +263,14 @@ TEST(EssentialMatrix, errorValue) {
}
//*************************************************************************
double error_(const Rot3& R, const Unit3& t) {
// Use two points to get error
Point3 a(1, -2, 1);
Point3 b(3, 1, 1);
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 a(1, -2, 1);
Point3 b(3, 1, 1);
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);
@ -283,18 +279,27 @@ 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());
HDexpected = numericalDerivative22<double, Rot3, Unit3>(error_, E.rotation(),
E.direction());
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;
E.error(a, b, 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));
}
/* ************************************************************************* */