feat: score based covariance estimation

[SakodaShintaro]

This pull request mainly adds three functions to estimate covariance in NDT.

• estimate_xy_covariance_by_Laplace_approximation
• estimate_xy_covariance_by_multi_ndt
• estimate_xy_covariance_by_multi_ndt_score

https://github.com/tier4/ndt_omp/pull/45/files#diff-bd4f8c20818118d63c4fd82470a937c0dd9962414fc67ace262046bc3ff4b136R16-R19

One that seems particularly promising is score-based covariance estimation.

In the method, after estimating the pose using NDT, only the score is calculated using NDT for about 10 points with a fixed offset, and the covariance is estimated as a weighted sum calculated from those scores.

For example, in places like tunnels, I have confirmed that there is a large covariance in the direction of travel.

When converted to the base_link coordinate and plotted the magnitude of covariance about AWSIM nishishinjuku data, it becomes as follows.

The speed is