Add new resampling policies

[glpuga]

Related to #57

Summary

• Refactors minimum motion check into a resampling policy
• Adds a policy that implements the "resampling_interval" parameter.
• Adds a policy that implements the "selective_resampling" parameters (WIP).

Checklist

• Read the contributing guidelines.
• Configured pre-commit and ran colcon test locally.
• Signed all commits for DCO.
• Added tests (regression tests for bugs, coverage of new code for features).
• Updated documentation (as needed).
• Checked that CI is passing.

[glpuga]

@nahueespinosa @ivanpauno this is open for feedback.

[nahueespinosa]

@glpuga This looks awesome! I just have a design question for now.

[ivanpauno]

Great work Gera!

I'm a bit concerned about the scalability of the proposal though.
See my review comments for concrete examples.

I understand though, this is not super easy to solve.
Ideally, I would have a ResampleIntervalPolicy, ResampleOnMotionPolicy and SelectiveResamplingPolicy classes that implement the same "interface", and another class ComposableResamplingPolicy that as well implements the same "interface" and allows registering multiple of the others resampling policies.
In that way, you could use one resampling policy in particular or a combination.

This is not so simply with the mixin though, and its harder considering that some of the resampling policies need access to other parts of the mixin.

Some concrete ideas:

• We could use the observer pattern for any component to observe motion updates.
  The resampling policies that need that will need to register there.
  Currently, the "update_motion()" method is part of the "MotionModel", but we could move it out of there and make also the motion model an "observer" of motion updates.
• Same concept could be applied to sensor measurements, though that's not needed here.
• Maybe instead of making the resampling policy a mixin, make it independient and it composes with the BootstrapParticleFilter (the component type will be a new template parameter).
  With that change, it's easier to make ResampleIntervalPolicy, ResampleOnMotionPolicy, SelectiveResamplingPolicy and ComposableResamplingPolicy all have the same interface.
  In the ros node, we always use ComposableResamplingPolicy, so the resampling policy will not change the final particle filter type.

[nahueespinosa]

Maybe instead of making the resampling policy a mixin, make it independient and it composes with the BootstrapParticleFilter (the component type will be a new template parameter).
With that change, it's easier to make ResampleIntervalPolicy, ResampleOnMotionPolicy, SelectiveResamplingPolicy and ComposableResamplingPolicy all have the same interface.

I think the ComposableResamplingPolicy is a good idea (although I would make that a mixin, not a template parameter).

[ivanpauno]

although I would make that a mixin, not a template parameter

The difference is that if it's in the mixin, then you still need a new particle filter type, even if you use the ComposableResamplingPolicy (because of the Mixin template parameter, everything needs to be added in ciabatta::mixin<...>).
If we use composition, you wouldn't need a different particle filter type when using a ComposableResamplingPolicy, you can just have the same one.

It makes sense to me to make the resampling policy a component of BootstrapParticleFilter, as it's the only place where you need to check if to resample or not.
It's true that different resampling policies may require to have access to different things, some of them to the motion model updates, some others to all the particles (e.g. SelectiveResamplingPolicy).
So it's also nice to have them as a mixin ....

[glpuga]

@ivanpauno @nahueespinosa thank you for the comments. Feel free to continue discussing; I'll be able to take a look late today, I'll catch up with your comments then.

[glpuga]

We could use the observer pattern for any component to observe motion updates.
The resampling policies that need that will need to register there.
Currently, the "update_motion()" method is part of the "MotionModel", but we could move it out of there and make also the motion model an "observer" of motion updates.

I walked this line of thought too, and eventually discarded it for the reasons I explained above, but the code is available to scavenge it if we want in a copy branch I created before changing direction:

• subscripton and emision: https://github.com/ekumenlabs/beluga/compare/glpuga/event_manager#diff-66681c52c1c2a347288d5052d876dde89f42589993fcec72ab62b86c13f48247R42
• event manager: https://github.com/ekumenlabs/beluga/compare/glpuga/event_manager#diff-5720c23eda60b33ab7eabda5c272fa0d26b1a633f490b7cfc0d12cc25bfdc361R23
• tests: https://github.com/ekumenlabs/beluga/compare/glpuga/event_manager#diff-9927cbe1d3759a694bf879c51d5a82e40ebaae50beee6a199bd2c2d55e19108aR66

[glpuga]

Ideally, I would have a ResampleIntervalPolicy, ResampleOnMotionPolicy and SelectiveResamplingPolicy classes that implement the same "interface", and another class ComposableResamplingPolicy that as well implements the same "interface" and allows registering multiple of the others resampling policies.
In that way, you could use one resampling policy in particular or a combination.

I think it can work, but at least for the three policies that we have now it would require:

• a way to communicate data from the main class and into the policies (e.g. the event subscription system from above to get motion updates)
• a way for policies to call arbitrary methods in the main class (e.g. access to particle weights). If we set an abstract interface that the main class needs to comply with, then we can give access to policies to the main class through the reference returned by this->self()

An itch with this is that, say a hypothetical policy is only meant to be used with a given motion model, and that they are coupled through a particular interface that only that motion model provides, then this design would not allow for this model/policy to be implemented in a straight way, because the virtual interface would have to be made to open access to a method that only one particular motion model provides in benefit of a single policy.

[glpuga]

@nahueespinosa @ivanpauno Thanks for looking, I'll update the PR tomorrow to address your first round of comments and lets iterate from there.

[ivanpauno]

a way for policies to call arbitrary methods in the main class (e.g. access to particle weights). If we set an abstract interface that the main class needs to comply with, then we can give access to policies to the main class through the reference returned by this->self()

I think it doesn't need to be abstract, it could be templated.

You would have something like

class MyResamplePolicy
{
  template<typename ParticleFilter>
  bool do_resampling(ParticleFilter& self) {...}
};

template<
  ...
  ResamplePolicy,
  ...
>
class BootstrapParticleFilter
{
  void resample() {
    if (resample_policy.template do_resampling(this->self())) {
    ...
    }
    ...
  }
...
private:
  ResamplePolicy resample_policy;
};

a way to communicate data from the main class and into the policies (e.g. the event subscription system from above to get motion updates)

Something like the above solve boths issues, as you also can access the motion updates that way.

---

I'm not sure if what I'm proposing above is a better solution honestly, but it would work.
I particularly like the fact that everything uses the same interface and you could easily combine then at runtime, or write a new policy that combines others manually.
I think that the main limitation of the proposal above, is that you always end up needing the runtime dispatching. Though it's not something that will hurt performance really. And that already solves the scalability issues, so I think both solutions are okay.

[glpuga]

@nahueespinosa @ivanpauno I updated the branch with the first round of review comments.

Some post-thoughts on the approach, something that is fine for now but may need rethinking is registering the resampler from their constructor:

• That makes them responsible for enabling and disabling their functionality based on configuration.
• That's fine for the amcl replacement node, but may or make not sense for a library of orthogonal modules that can be reused to build other localization filters, where they might be active just because they were added to the mix.
• Also currently ResampleOnMotionPolicy and ResampleIntervalPolicy add themselves unconditionally, but SelectiveResamplingPolicy only does it if selective_resampling is true in the configuration. That's because that's how the configuration of amcl does it. That's asymmetrical, though; either all three should only activate themselves conditioned on configuration, or none of them should if they do it themselves.

[nahueespinosa]

@glpuga This is fantastic! I did another pass.

[ivanpauno]

Feel free to ignore this, but I came up with an alternative that I wanted to give it a try.
It solves the issue comented before:

Some post-thoughts on the approach, something that is fine for now but may need rethinking is registering the resampler from their constructor:

I basically added a feature to ciabiatta to allow "combining" mixins into one.
In that way, you need only on top level "ResamplePolicy" in the mixin, which can be directly one of the interval, on motion, selective, or an implementation of a combination of them (using the new ciabatta feature).

Ciabatta diff: glpuga/add_resampling_interval_policies...ivanpauno/add_resampling_interval_policies#diff-aa8386abc68af06e069cccdabc6bc7a9f0a8b5524b82731249d99dfc9added62
One possible implementation of combining policies: glpuga/add_resampling_interval_policies...ivanpauno/add_resampling_interval_policies#diff-6498356956b531163813409e6240f087399239bdafc4c8a800b867b26d6f363a

It's kinda tricky, but I thought it was an interesting approach, and the tricky details are hidden in ciabatta.

[nahueespinosa]

I came up with an alternative that I wanted to give it a try.

I think that the implementation is cool. It adds a level of complexity though, so I'd like to discuss the original concerns first here.

Some post-thoughts on the approach, something that is fine for now but may need rethinking is registering the resampler from their constructor:

• That makes them responsible for enabling and disabling their functionality based on configuration.
• That's fine for the amcl replacement node, but may or make not sense for a library of orthogonal modules that can be reused to build other localization filters, where they might be active just because they were added to the mix.

I think it does make complete sense for them to be active just because they were added to the mix. In the end, the goal that I want to pursue is: "here is a couple of combinations that work for localization, if you want something different, define your own mix, take the mixins you want from beluga, add some custom ones and compose your own type at compile time". I have a change in the works for ciabatta that will allow us to do something like:

using CustomLocalization = ciabatta::mixin<
  Component1,
  Component2,
  ciabatta::curry<Component3, TemplateParameterForComponent3>::mixin,
  ciabatta::provides<CustomLocalizationInterface>::mixin>;

Also currently ResampleOnMotionPolicy and ResampleIntervalPolicy add themselves unconditionally, but SelectiveResamplingPolicy only does it if selective_resampling is true in the configuration. That's because that's how the configuration of amcl does it. That's asymmetrical, though; either all three should only activate themselves conditioned on configuration, or none of them should if they do it themselves.

I'd go for adding an "enabled" parameter for the three of them, setting them to true unconditionally in beluga_amcl.

[ivanpauno]

It adds a level of complexity though

It adds complexity to ciabatta, but I think it's removing it from the particle filter implementation.
Currently, you cannot simply apply one resample policy to the particle filter without also applying the "dynamic dispatch" part.

"here is a couple of combinations that work for localization, if you want something different, define your own mix, take the mixins you want from beluga, add some custom ones and compose your own type at compile time"

I agree with that idea, but I think that's exactly what this PR is not allowing.
You cannot escape from having a ResamplingPolicyPoller, that's the only way of combining resample policies.
In the alternative, you can use OnMotionResamplePolicy directly, without the need to have any runtime dispatch or using the new ciabatta trick at all.

so I'd like to discuss the original concerns first here.

To add to the previous concerns, the current "ResampleIntervalPolicy", "ResampleOnMotionPolicy", "SelectiveResamplingPolicy" are implemented using the mixin pattern, but are not a really mixin conceptually.
They're not extending the functionality of the concrete class at all, they are simply registering a callback in a mixin that's actually extending the functionality to the concrete class.
In that case, we could implement the resample policies to be registered as a callback, and then have a wrapper that also allows them to be used directly as a mixin, e.g. godbolt.

i.e. I think mixins are meant to provide orthogonal functionality.
The resample policies, when combined, are not orthogonal.
That's why I think having many resample policies as template parameters of the concrete mixin is a bit weird.

[nahueespinosa]

To add to the previous concerns, the current "ResampleIntervalPolicy", "ResampleOnMotionPolicy", "SelectiveResamplingPolicy" are implemented using the mixin pattern, but are not a really mixin conceptually.
They're not extending the functionality of the concrete class at all, they are simply registering a callback in a mixin that's actually extending the functionality to the concrete class.
In that case, we could implement the resample policies to be registered as a callback, and then have a wrapper that also allows them to be used directly as a mixin, e.g. godbolt.

@ivanpauno I love this idea 100%. The template Concrete type parameter in the policies is super clever and it fits super well with the library as a whole. I added a template CombinedDoSomethingMixin here: https://godbolt.org/z/5654zh88G.

@glpuga Please take a look when you can, this is great!
Regarding solving the issue of enabling and disabling the selective resampling policy in beluga_amcl, we can use the same approach that we will use for selecting sensor and motion models. We can a have a combined mixin including that policy and a combined mixin without it.

[glpuga]

@ivanpauno @nahueespinosa I like the new proposal! please check the implementation now when you have a moment.

[nahueespinosa]

@glpuga Awesome! Left a couple minor comments and super minor documentation suggestions.

[nahueespinosa]

I just applied the punctuation and minor documentation comments using the GitHub commit batch interface.

[glpuga]

Latest commented addressed, I'll wait to have @ivanpauno ok to merge.

[ivanpauno]

LGTM!
Thanks for all the iterations @glpuga !!!!