Add nD-NDT maps to Beluga

[hidmic]

Feature description

Connected to #93. Before we can implement NDT based sensor models in Beluga, we need an efficient data structure for NDT maps of arbitrary dimension.

Implementation considerations

https://github.com/cogsys-tuebingen/cslibs_ndt may be of interest.

[serraramiro1]

What about https://github.com/Tessil/robin-map ? This is the library Kiss-ICP uses and I had very good results with.
It contains std::unordered_map drop-in replacements depending on your needs.

Here're some benchmarks:
https://tessil.github.io/2016/08/29/benchmark-hopscotch-map.html

[hidmic]

@serraramiro1 how would you lay out NDT maps using those data structures? I imagine we will need some form of spatial indexing for speed when we get to #311.

[serraramiro1]

@hidmic
I'm not completely sure if we need spatial indexing here.

Spatial indexing is usually good for complex queries, like intersections between complex shapes.

In this case we can leverage the map being discretized.
I'm thinking of:

NDT map represented as Discretized cell -> Normal Distribution hash map.

Then for getting likelihood for a point {x, y} is as easy as

discretized_cell = {floor(x / resolution), floor(y / resolution)};
if discretized_cell not in map:
      return MINIMUM_LIKELIHOOD
return map[discretized_cell].likelihood(x,y)

Hashing integers is almost free and I don't think doing spatial indexing will outperform this.

I might be overlooking something tho.

Thoughts?

[hidmic]

IIRC NDT maps also use some shifted copies to deal with boundary effects, but yeah, that can be sorted out with cell hashes. Eventually we may want to be smart about how we run over the pointcloud to maximize cache efficiency (hash maps are O(1) but memory I/O is not). I'll try and take a look at at robin_map to see what it can offer in that sense. For now, the approach makes sense to me if @nahueespinosa is OK with the extra dependency (has is it been released anywhere already?).

[nahueespinosa]

If the idea is to use the drop-in replacements for std::unordered_map, then I would support implementing a class with the necessary accessor methods where the underlying map type is a template parameter. This way we don't have to add the extra dependency to beluga core, and instead defer that to the concrete implementation packages.

[hidmic]

As of #336 and #338, we kinda have a first 2D variant. I'll keep this one open until we land a 3D variant.

[serraramiro1]

The 3D variant was introduced with #422