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

4
gtsam/geometry/EssentialMatrix.h
View File

@ -161,7 +161,9 @@ class EssentialMatrix {
/// epipolar error, sampson /// epipolar error, sampson
GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, 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) { double error_(const Rot3& R, const Unit3& t, const Point3& a, const Point3& b) {
// Use two points to get error
Point3 a(1, -2, 1);
Point3 b(3, 1, 1);
EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t); EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t);
return E.error(a, b); return E.error(a, b);
} }
TEST(EssentialMatrix, errorJacobians) { TEST(EssentialMatrix, errorJacobians) {
// Use two points to get error // Use two points to get error
Point3 a(1, -2, 1); Point3 vA(1, -2, 1);
Point3 b(3, 1, 1); Point3 vB(3, 1, 1);
Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
Point3 c1Tc2(0.4, 0.5, 0.6); Point3 c1Tc2(0.4, 0.5, 0.6);
@ -283,18 +279,27 @@ TEST(EssentialMatrix, errorJacobians) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix13 HRexpected; Matrix13 HRexpected;
Matrix12 HDexpected; Matrix12 HDexpected;
HRexpected = numericalDerivative21<double, Rot3, Unit3>(error_, E.rotation(), Matrix13 HvAexpected;
E.direction()); Matrix13 HvBexpected;
HDexpected = numericalDerivative22<double, Rot3, Unit3>(error_, E.rotation(), HRexpected = numericalDerivative41<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(),
E.direction()); 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; Matrix15 HEexpected;
HEexpected << HRexpected, HDexpected; HEexpected << HRexpected, HDexpected;
Matrix15 HEactual; Matrix15 HEactual;
E.error(a, b, HEactual); Matrix13 HvAactual, HvBactual;
E.error(vA, vB, HEactual, HvAactual, HvBactual);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(HEexpected, HEactual, 1e-5)); EXPECT(assert_equal(HEexpected, HEactual, 1e-5));
EXPECT(assert_equal(HvAexpected, HvAactual, 1e-5));
EXPECT(assert_equal(HvBexpected, HvBactual, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */