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]

These comments might be better in the respective pre-integration classes. Also, it's hard to understand exactly what is meant.

Maybe to clarify, I use

• left perturbation as exp(xi)*T (which is, ironically, right-invariant)
• right perturbation as T*exp(xi) (which is left-invariant)

[JzHuai0108]

Yes. Will move them to their respective class headers. I am comfortable with left perturbation exp(xi)*T notations. These comments are not easy to understand since they refer to evidence in other files. I put them here mainly for your reference, and will simplify them as I move them around.

[dellaert]

Ah, so you're saying the manifold pre-integration was wrong? Not the tangent pre-integration?

And I am right in interpreting that you see the manifold pre-integration as using the right-invariant error instead of the left-invariant?

Please forgive my questions: I have relatively little time today to take the time I should really take to figure it out, so hoping the dialogue will make things easier.

[JzHuai0108]

I think the manifold preintegration's covariance/Jacobian computation is problematic, but the manifold preintegration's value is OK. The original code of manifold preintegration computes the preint measurement covariance using the error definition: 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}; but computes its bias Jacobians using the error definition:
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}. Therefore, there is an inconsistency between covariance and Jacobian of the preint measurement.

To me, the tangent preintegration is OK in value and Jacobians.
I think all error states in gtsam are left-invariant based on my observations.

[JzHuai0108]

In writing the extended abstract, I found some inconsistency in my derivations :(. I will double check it in the coming days.