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

using correct jacobian computation for calibration

release/4.3a0
Ayush Baid 2021-06-10 12:21:32 -07:00
parent 2e69d09732
commit 2e8bfd60fc
1 changed files with 11 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/slam/EssentialMatrixFactor.h
View File

@ -80,7 +80,7 @@ class EssentialMatrixFactor : public NoiseModelFactor1<EssentialMatrix> {
const EssentialMatrix& E,
boost::optional<Matrix&> H = boost::none) const override {
Vector error(1);
error << E.error(vA_, vB_, H);
error << E.error(vA_, vB_, H, boost::none, boost::none);
return error;
}
@ -367,20 +367,22 @@ class EssentialMatrixFactor4
Vector3 vA = EssentialMatrix::Homogeneous(cA);
Vector3 vB = EssentialMatrix::Homogeneous(cB);
Matrix13 error_H_vA, error_H_vB;
Vector error(1);
error << E.error(vA, vB, H1, H2 ? &error_H_vA : 0, H2 ? &error_H_vB : 0);
if (H2) {
// compute the jacobian of error w.r.t K
// error function f = vA.T * E * vB
// H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
// error function f
// H2 = df/dK = df/dvA * dvA/dK + df/dvB * dvB/dK
// where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
// and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
*H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
*H2 = error_H_vA.leftCols<2>() * cA_H_K
+ error_H_vB.leftCols<2>() * cB_H_K;
}
Vector error(1);
error << E.error(vA, vB, H1);
return error;
}