Beam skipping in likelihood field model

[nahueespinosa]

Description

Beam skipping is a heuristic implemented in the likelihood_field_model_prob from nav2 that can potentially help with dynamic obstacles. The idea is to ignore the beams for which the majority of the particles do not match the map, this would prevent the correct particles from getting down weighted due to unexpected obstacles such as humans.

To do that, the algorithm first goes through all the particles and calculates the percentage of particles for which a particular beam matches the map, if it is below a certain threshold, it ignores that beam when calculating the weight of each particle in a second step. If the number of beams to be skipped falls below a certain threshold, it will disable beam skipping for that iteration and integrate all beams.

Definition of done

Beam skipping feature implemented in Beluga.

[DPR00]

Hi there @nahueespinosa @hidmic! should we create a new sensor model type called LikelihoodFieldProb as it is done in NAV2 AMCL? At first glance, it seems the "appropiate" thing although LikelihoodField and LikelihoodFieldProb have a lot of code in common. :thinking: I am more prone to use the same LikelihoodField model and add new paramateres, but I have to spent some time looking the code to see if that's possible. Or maybe an interface? WDYT? Suggestions are always welcome :sweat_smile:

[nahueespinosa]

@DPR00 From the looks of it, these are the inputs of the algorithm:

• the likelihood field
• the particle set
• some configuration parameters

One of these two problems need to be solved here:

1. The sensor model does not have direct access to the particle set.
2. The rest of the world does not have access to the likelihood field.

In my view, the best solution would be to decouple the likelihood field from the likelihood field sensor model, and create a new free-function that implements the beam skipping algorithm.

A bit of pseudo-code:

auto likelihood_field = LikelihoodField{map};
auto sensor_model = LikelihoodFieldModel{likelihood_field, ...};
...

for (auto&& [measurement, ...] : data) {
  auto filtered_measurement = skip_unlikely_beams(particles, measurement, ...);

  particles |= beluga::actions::propagate(...) |
               beluga::actions::reweight(sensor_model(std::move(filtered_measurement)) |
               ...
}

As a side note, they don't seem to use the cubic formula to aggregate probabilities in this model, so we should look into that as well.

[hidmic]

As a side note, they don't seem to use the cubic formula to aggregate probabilities in this model, so we should look into that as well.

They use log weights. I'd be inclined towards keeping the LikelihoodFieldModel for 2D in the same class, using the parametrization and abstractions discussed above, and deferring the weight discussion for #153. In my mind, we should eventually define a beluga::LogWeight type, and have weight construction, aggregation, and normalization be bound to the weight type. Perhaps it's overkill and we should work all weights in the same space (linear, log, whatever), but leaving the door open sounds reasonable to me.

[glpuga]

I wonder if, instead of implementing it, we should evaluate whether this actually makes a difference in performance. Is there empirical or bibliographic support for this feature? Does people use this?

The multi-parameter statistical parameter for the beam moel models unmapped obstacles to some extent. The likelihood model lacks this term because it "sees through".

At first sight, pre-selecting measurements that confirm the proposal distribution with no other model to support this sounds like selection bias to me.

[hidmic]

I wonder if, instead of implementing it, we should evaluate whether this actually makes a difference in performance. Is there empirical or bibliographic support for this feature? Does people use this?

There's neither other than being a long standing feature in Nav2. We also won't know whether it makes a difference in performance until after we have tried it, but unlike most situations we could use Nav2 AMCL as the "reference" implementation for a quick validation.

At first sight, pre-selecting measurements that confirm the proposal distribution with no other model to support this sounds like selection bias to me.

It does come off as a hack to mitigate the numerical issues that motivate likelihood aggregation via sums of cubes. LikelihoodFieldProb accumulates log weights but then normalizes in linear space so it's not entirely out of the woods. Theoretically speaking, I'm with you, it's all nonsense, but so is summing cubes and it works :man_shrugging:

[hidmic]

All in all, in general and from a process perspective, I'd like to have a standard procedure for validating improvement hypothesis such as this. Landing https://github.com/Ekumen-OS/lambkin/pull/93 and #415 will get us one step closer to that.

[hidmic]

There's neither other than being a long standing feature in Nav2.

FTR I unearthed https://github.com/ros-planning/navigation/pull/238. Those numbers they promised never saw the light of day, and yet it was merged, so I can only assume author and maintainer knew each other.

[glpuga]

but so is summing cubes and it works

Indeed, but notice that we know the sum of cubes works because it's the default and therefore we can infer people have been using it for years with no complains, even if it lacks rigorous support.

That's not so clear for beam skipping; I can't really tell how many people actually use this, and the node defaults to not doing it, which maybe also speaks for how broadly relevant this feature is.

I think doing a nav2-vs-nav2 sanity check would be a nice thing to do before embarking on an implementation that might require changes to API to get to work.

[hidmic]

we know the sum of cubes works because it's the default and therefore we can infer people have been using it for years with no complains

I'm with you in everything but the no complains bit :eyes:

Anyways, this goes beyond this feature. We don't have a clear vetting procedure for feature requests, so we go pursue them all and defer that choice to the user. In a perfect world, I'd first rank them based on theoretical soundness, then enforce data-driven checks. I wouldn't want to stall development until we have that though.

I think doing a nav2-vs-nav2 sanity check would be a nice thing to do before embarking on an implementation that might require changes to API to get to work.

:+1: I fully agree with that. FYI @DPR00.

[glpuga]

FTR I unearthed ros-planning/navigation#238.

Interesting. @mikeferguson by any chance do you remember any context about the beam skipping feature in https://github.com/ros-planning/navigation/pull/238 ?

[mikeferguson]

That's a long time ago - but looking at the repo it came from, and who was involved, I think this was used in the Kuri robot? I'm guessing there was a side conversation, but I don't have any metrics to provide on whether this does much to improve performance.

[glpuga]

@DPR00 the base skeleton benchmark is already up and working https://github.com/Ekumen-OS/lambkin/pull/116 , feel free to take over.

Notes:

• At first sight, there's no difference betweeen using beam skipping or not, performance is the same with this limited dataset. I only enabled beam skipping, but I did not change the defaults for other parameters related to it.
• There is, however, a dramatic performance difference between the likelihood_field and likelihood_field_prob models, with the latter having between 1/2 and 1/3 the APE in my limited run attached below.
• In one of the runs likelihood_field_prob diverged near the end, but it's sibling likelihood_field_prob+beam_skipping did not.

report.pdf

nav2_amcl_likelihood_pose

nav2_amcl_likelihood_prob_pose

nav2_amcl_likelihood_beam_skip_pose

Recommendations:

• [ ] Replicate my run, check you get similar results (CPU usage may vary if you use a different host).
• [ ] Validate the numbers in the report by running EVO directly on the output bagfile, to make sure the performance of likelihood_field_prob is not a bug in the report generation code (JIC, but the evo plots generated by lambkin are consistent with the numbers in the report).
• [ ] Validate that the parameter files we are loading are effectively defaults + changes related to the sensor model + changes related to beam-skipping.
• [ ] Validate that the nodes are running with the parameters we intend, looking at the parameters.yaml file in the output folder of each benchmark.
• [ ] We may want to extend the benchmark with additional bagfiles. The original simulated datasets are 24hs long, so we can create up to 24 1hr slices.
• [ ] We'll want to add a real-robot benchmark to see if the performance differente translates there too. I think the TorWIC SLAM is the best one for this. Notice that no real-bot dataset we have will make any difference for beam-skip-vs-non-beam-skip because every obstacle will be part of the map; they can only compare likelihood-vs-likelihood-prob.