Fix/covariances

[JzHuai0108]

1. straighten the ManifoldPreintegration cov computation, making it consistent with the Jacobians;
2. unit test covariance of PreintegratedImuMeasurements and PreintegratedCombinedMeasurements derived from ManifoldPreintegration or TangentPreintegration by comparing with ImuEKF;
3. unit test that preintegrated measurements derived from ManifoldPreintegration and TangentPreintegration have almost identical cov as the residual is defined the same;
4. largely fix the NEES computation.

[dellaert]

Many thanks for working with me on this. This is very important to me.

[dellaert]

OK, I went through this more carefully. Not fully reviewed yet, but

• The key point should be stated more explicitly in the PR description: “old manifold preintMeasCov lived in the moving $T_{\Delta}$ chart; new one lives in the increment chart consumed by biasCorrectedDelta/predict.” do you agree with that assessement?
• UpdatePreintegratedSlow is extremely useful for testing, but I would probably not leave it as public API?
• Would it be possible to add one regression test with non-identity body_P_sensor? That post-multiplication of B,C after the chart transform is an easy place for subtle mistakes.

[JzHuai0108]

1. I am quite rusty about chart. :) I will put the change to ManifoldPreintegration in plain calculus terms.

Let's denote the right perturbation dXn on the NavState X_ij's manifold, i.e.,
dXn = [\delta \phi_ij, \delta p_ij, \delta v_ij] where R_ij = \hat{R}_ij Exp(\delta \phi_ij)
p_ij = \hat{p}ij + R_ij \delta p{ij} and v_ij = \hat{v}ij + R_ij \delta v{ij}.

For ManifoldPreintegration, for covariance propagation, the perturbation on deltaXij(preint X_ij) is dXn,
the right perturbation of the NavState manifold, as validated by the unit tests on NavState::retract);
but for jacobian computation, the perturbation on deltaXij (ie, preint X_ij) was actually defined as
dXt = [\delta \phi_ij, \delta p_ij, \delta v_ij] where R_ij = \hat{R}_ij Exp(\delta \phi_ij)
p_ij = \hat{p}ij + \delta p{ij} and v_ij = \hat{v}ij + \delta v{ij},
as validated by the unit test on delPdelBiasAcc().

For ImuFactor or CombinedImuFactor, the residual is defined by PreintegrationBase::computeError and
its perturbation is coincident to dXt (up to a right Jacobian of a small angle ~ identity).

For TangentPreintegration, the perturbation on deltaXij is dXt, the 3D perturbation of the rotation tangent space +
3D perturbation of the translation part + 3D perturbation of the velocity part,
as validated by the matrix A_k in the ImuFactor.pdf and the unit test on preintegrated_H_biasAcc()

So for TangentPreintegration, the covariance of the residual equals to the preintMeasCov of TangentPreintegration.
But for ManifoldPreintegration, we need to rotate the position and velocity blocks of the preintMeasCov to get the residual cov.
Otherwise, we can update the cov propagation of manifold preintegration to make it use dXt. This is what we do in the pull request (see ManifoldPreintegration::update()) to keep the API change minimal.
If you are interested, expand the commits of the pull request, you will notice the former approach, but it has been overridden by the latter approach.

Overall, I think I agree with the assessment based on my understanding of charts.

2. It has been moved to the test file since it was static.
3. A test on Jacobians B and C is added with non-identity body_P_sensor. Note Jacobian A is tested by the covariance comparison with EKF (see integrateMeasurement() and RunCombinedVsEkf9D()), and the Jacobian comparison to the UpdatePreintegratedSlow.

[dellaert]

Many thanks for your contribution! @scottiyio and I will do one last NEES check on another branch and then merge.

[JzHuai0108]

I just added the last key piece. Now the fix is complete. Also retouch the comments for clarity.