Explore robust pose estimation

[hidmic]

Feature description

Recent benchmarks have shown that, while Beluga AMCL and Nav2 AMCL are on par on average, Beluga's worst case performance (i.e. $sup \mathrm{APE}$) is subpar compared to good ol' AMCL.

One possible cause for this discrepancy, and the one I entertain the most, are outliers. Outliers would skew Beluga's pose estimation, which relies on sample statistics only, whereas Nav2 AMCL's clustering algorithms may naturally exhibit some resistance to them.

While we could simply work our way forward with #258, I think there is value in exploring methods purposely designed for outlier rejection, and in particular, those from the robust statistics domain.

Implementation considerations

Beluga's design is advantageous for this: robust pose estimation support amounts to adding a new estimator mixin.

[hidmic]

I dived into this a couple weeks ago. Didn't get as far as to write code, but I did came up with some notes and a plan of action:

• While we could rely on clustering for this (e.g. kNN in SE(2) like Nav2 AMCL does, fitting mixture models using EM and picking the most likely mode, etc,), it turns out there is an entire field of statistics devoted to robust location and dispersion estimates (location and dispersion as a generalization of mean and variance).
• For robust location and dispersion, Minimum Covariance Determinant (or MCD) is king. It's also incredibly slow, as it has to try every possible combination of samples and perform as many matrix inversions. There is a Fast MCD variant that combines stochastic sampling and numerical heuristics to speed things up, but from what I gather it cannot run real-time either.
• Then I came across the OGK method. It has been experimentally shown (and only experimentally AFAIK) to approximate the MCD estimate, and it is way cheaper to compute. There are some very informative benchmarks in the Robust Estimates of Location and Dispersion for High-Dimensional Datasets article that first proposed the method. Fun fact: Ricardo Maronna, its author, is (or was?) a PI at UNLP 😅. Do you know him @glpuga?
• The OGK method may still not be real-time ready though. It computes lots of medians. Standard quicksorting to calculate medians is an O(n log n) algorithm on average. Quickselecting, on the other hand, is an O(n) algorithm on average.
• If the above still won't do, we have the remedian estimator, that asymptotically converges to the median. It's basically a recursive median of medians on a data stream. It was designed to work on large data sets (e.g. larger than system memory), which is not our use case, but it is because of it that it comes with a very interesting feature: fixed, user-defined storage requirements. That feature would allow us to make smart choices, like picking storage size and alignment to ensure cache locality and maximize SIMD throughput. Add something like https://github.com/intel/x86-simd-sort to the mix and we should be close to the physical speed limit on modern hardware.

So, an OGK estimator and qselect medians would be a good start IMO.

[hidmic]

For further reference, section 6.8 of Robust Statistics: Theory and Methods (with R) book, also co-authored by Ricardo Maronna, covers several competing methods. I only skimmed through it though.

[glpuga]

This is ok by me, but we should first test the hyphotesis about what the problem is before looking for performant solutions for it. I would definitely not want spend a couple months implementing an unproven way to solve a problem we are not sure we have.

While we could rely on clustering for this (e.g. kNN in SE(2) like Nav2 AMCL does, [...]

I would still start with this before trying anything else. This is already far better than what we have and it's been extensively tested in the use cases we are mainly interested in.

Afterwards, let's keep in mind that "real-time" is in the eyes of the beholder. It's the users that strike the balance between their application and the resources they throw into it. Most likely a "slow" solution will not be the default for beluga_amcl, but can still be a tool in the beluga kit.

Ricardo Maronna, its author, is (or was?) a PI at UNLP 😅. Do you know him @glpuga?

I'm afraid not. Looks like he works at Ciencias Exactas.

[hidmic]

we should first test the hyphotesis about what the problem is before looking for performant solutions for it

Absolutely. That is why this is an exploration. I stared down the rabbit hole only to make sure this isn't a dead-end (as it would have been if we implement something like MCD only to realize it takes seconds to yield an estimation).

I would still start with this before trying anything else. This is already far better than what we have and it's been extensively tested in the use cases we are mainly interested in.

I'm on the fence about this. Doing exactly what Nav2 AMCL does is the low hanging fruit and it ensures we won't do worse. It also means we won't do any better (in terms of localization performance, we can still marginally improve in terms of computational performance).

Afterwards, let's keep in mind that "real-time" is in the eyes of the beholder. It's the users that strike the balance between their application and the resources they throw into it.

That's technically true, but if your solution is an order of magnitude slower for typical operation rates (1-10 Hz?) on modern CPUs (i5, i7, i9, Ryzen 5, Ryzen 7) then it is highly likely that it will be useless in most cases.