Add brute force multi-resolution matcher

[hidmic]

Feature description

Whether it's for localization recovery or as a form of informed roughening (some comparatively older SLAM systems used to do this for loop closure), it can be useful to globally search for a likely state hypothesis in a gridmap. There are many ways to do this, with different performance tradeoff.

A brute force, multi-resolution (aka pyramid) grid matcher is a good start, and immediately useful to MH-AMCL.

Implementation considerations

Do we need another grid type for this, one that can accumulate scans? 🤔

[hidmic]

FYI @JesusSilvaUtrera

[JesusSilvaUtrera]

@hidmic I have been thinking about the implementation of this, and wanted to share some thoughts before starting to develop, to know your opinion.

Given that the implementation of the "multi-resolution map matcher" just uses an OccupancyGrid in the constructor, I have thought of creating the following:

• First, a MapPyramid (or MultiResolutionMap, names are up to discussion of course), that takes in either a BaseLinearGrid2 or a BaseOccupancyGrid2 (I suppose the first to make it more general, but I am not sure if we will require it to be an OccupancyGrid type for it to work), storing a vector of grids (each one for a level) and having a downsample(or half_scale) method to go from highest to lowest level. This will be stored in beluga/sensor/data.
• Then, the abstract interface BaseMapMatcher, taking a map representation and a scan, and returning a set of scored poses. It will have templated Pose and Point arguments, and a match method that takes the MapPyramid and the scan to generate the possible weighted transforms (pose + weight). This will be stored in beluga/sensor (or maybe in beluga/utility given that it's not a sensor per se?).
• Finally, the default implementation for the abstract interface, the BruteForceMapMatcher (again, names are provisional) that uses the implementation from MH-AMCL (at least at first thought, I will review it to see if something is not suitable). This will be stored in beluga_ros directly.

I will keep thinking and share if I change anything, but feedback is appreciated here.

[hidmic]

@JesusSilvaUtrera that is an interesting take. A few thoughts and suggestions:

• Take a look at ValueGrid for ideas on how a pyramid could be implemented. I'd think that the only requirement on the initial grid is for it to be dense.
• Consider separating the pyramid data structure from its construction. There are many ways in which you could build that pyramid ie. different scaling factors, different pooling expressions.
• As an alternative to abstract interfaces, consider a brute_force_scan_match(const PyramidT&, const LaserScanT&) -> std::vector<std::tuple<WeightT, Sophus::SE*>> function returning a collection of tuples (or structs) with weighted transforms.
• About occupancy vs non-occupancy. I'd suggest we have the matcher take an extra callable to decide what is a match. That way we can encapsulate how the algorithm decides there was a hit: maybe it's just checking it's occupied, maybe it's (fuzzy?) thresholding. We can have useful overloads with predefined operators to help the user.