Add NDT sensor model

[serraramiro1]

Proposed changes

Adds a new sensor model, based on NDT transforms, that performs importances weighting by transforming the measurements

Type of change

• 🐛 Bugfix (change which fixes an issue)
• 🚀 Feature (change which adds functionality)
• 📚 Documentation (change which fixes or extends documentation)

Checklist

Put an x in the boxes that apply. This is simply a reminder of what we will require before merging your code.

• Lint and unit tests (if any) pass locally with my changes
• I have added tests that prove my fix is effective or that my feature works
• I have added necessary documentation (if appropriate)
• All commits have been signed for DCO

Additional comments

Anything worth mentioning to the reviewers.

[serraramiro1]

@hidmic PTAL when possible, it's still a draft but might be good to get the review process going.

A few concerns I have.

Citing the NDT MCL paper:

In practice we approximate Eq. 4
by finding the cell from the map that corresponds to the mean
µ¯i = Rkµi + tk and search the local neighborhood in order
to find the closest normal distribution to µ¯i

I'm not doing nearest neighbor search here, but we could preprocess the grid to make that work or modify the conversion grid <-> NDT map script to take that into account, likelihoods fall so abruptly when you go away from the mean that I don't expect it to matter much but we would certainly have some boundary effects with the current approach.

The way they're suggesting the scaling of the likelihood with d1 and d2 I don't love.
Likelihoods are not bounded, and can be any positive number. The current version would favor perfect hits to narrower NDT cells to perfect hits to cells with bigger covariances..

I'd like to try a different version where

weight = unscaled_likelihood_at(measurement) / unscaled_measurement_at(mean)

This way, our weights would have an upper bound of 1 and would make our lives easier.

Also I'd consider using the log likelihood instead of the likelihood to make the weighting function less spiky and prevent us from giving poor scores to very-close hits to a narrow distribution.

[hidmic]

I'm not doing nearest neighbor search here, but we could preprocess the grid to make that work or modify the conversion grid <-> NDT map script to take that into account,

IIUC nearest neighbors here is just a performance optimization, so it's OK for a start if we don't do it. I do wonder about the detour in the algorithm from what Sarineen et. al. propose. We are not applying NDT to the measurement like Biber et. al. describe (i.e. discretizing space, fitting normal distributions within each cell) before computing likelihoods. Why?

likelihoods fall so abruptly when you go away from the mean that I don't expect it to matter much but we would certainly have some boundary effects with the current approach.

And that can be problematic, numerically speaking. Working with degenerate, point-mass distributions won't make it any better (like we do here instead of using NDT in full). Even working with NDT may not be enough. Both Biber et. al. and Schulz et. al. suggest having multiple map layers, offseted to deal with boundary effects. We are not doing that either.

The way they're suggesting the scaling of the likelihood with d1 and d2 I don't love.

Yeah, there's some handwaving to those scalings. $d_2$ I can understand though. By artificially inflating covariances you can compensate for the limited sample size issues (i.e. the issues that particle filters have when measurement likelihood distributions are too narrow).

Likelihoods are not bounded, and can be any positive number. The current version would favor perfect hits to narrower NDT cells to perfect hits to cells with bigger covariances..

I don't follow you here.

This way, our weights would have an upper bound of 1 and would make our lives easier.

We do normalize though, typically after reweighting.

Also I'd consider using the log likelihood instead of the likelihood to make the weighting function less spiky and prevent us from giving poor scores to very-close hits to a narrow distribution.

+1. That's something I've been wanting to try for a while. I have a strong suspicion that much of the heuristic that the other sensor models rely on when computing likelihoods can be traced back to numerical issues. Log likelihood and log odds representation may help. I would defer to a different PR though.

[hidmic]

Second pass. Overall looks reasonable.

[serraramiro1]

@hidmic ready to re-review.
Note that I couldn't fully extend this to 3D because of sparse grids being only-2D.
Making them support NDimensional data would involve going back to BaseGrid2 and I rather not go that deep for now.

I'll create a ticket for this effort and leave it here if you're OK with it.

[hidmic]

Overall LGTM. The coverage check is in a weird state.