Optimize beam sensor model runtime performance

[glpuga]

Proposed changes

Minor optimizations to the tightest execution loops in the beam sensor model.

While these changes do improve performance a bit, they barely make a difference in the performance disadvantage we have against Nav2 AMCL. We are still missing something much bigger than these optimizations.

Type of change

• 🐛 Bugfix (change which fixes an issue)
• 🚀 Feature (change which adds functionality)
• 📚 Documentation (change which fixes or extends documentation)

Checklist

Put an x in the boxes that apply. This is simply a reminder of what we will require before merging your code.

• Lint and unit tests (if any) pass locally with my changes
• I have added tests that prove my fix is effective or that my feature works
• I have added necessary documentation (if appropriate)
• All commmits have been signed for DCO

Additional comments

These changes change the performance profile, as seen through perf from this:

to this:

Note: These flamegraphs assume the changes in #199 have already been merged, since there are grid optimizations that are common to both Likelihood and Beam.

Notice that relative to the unmodified the beluga::Bresenham2i::Line block of code (that had no changes done to it, neither in performance per execution nor in total number of executions), the overall time spent in the importance_weight function seems to have reduced significantly.

Notice also the removal of the second stack call tower on the right, which appears to be related to queuing in the MessageFilter.

However, these changes barely changed the cpu usage profile:

And the change is barely noticeable in the difference against Nav2 AMCL.

While it's still possible our implementation of Bresenham is less peforming than Nav2's, even if we somehow reduced the tracing runtime cost to 0 with some magic implementation, that would still get us to perform at basically the same level as Nav2 amcl. To me that indicates that the problem is somewhere else.

I suspect we are actually doing more work than Nav2, but I haven't been able to find any proof of that.

Further work is still needed.

[hidmic]

LGTM

[nahueespinosa]

@glpuga Left two super minor comments. It is also worth noting that RelWithDebInfo reduces the optimization level to -O2 instead of -O3, so flamegraphs might not be as good at comparing micro-optimizations changes like these.

Microbenchmarks using googlebenchmark compiled in Release mode may be better at detecting true progress.