gicp Jacobian

[TianfaYao]

This work is so great!
The covariance calculation part contains T, so should the correlation term be reflected in the Jacobian matrix, but the code seems to treat this part as an ordinary covariance matrix,or the correlation approximation is used here ?

[koide3]

Yes, it is an approximated way; the covariances of source points are fixed at the linearization point and do not change during cost evaluation. I implemented an exact version with covariance drivatives, but it didn't improve accuracy nor stability while making optimization significantly slow. If you want, you can try it at the following link:

https://github.com/SMRT-AIST/fast_gicp/blob/87cd6288d14bd155e8b7a2144f68bb5246aecc52/include/fast_gicp/gicp/gicp_loss.hpp