Speed up speckle filter implementation

[erwinbonsmatopic]

These changes improve performance by a factor three.

Summary of changes:

• Do not allocate dynamic memory in each update invocation
• Speed up loop end-condition checks
• Algorithm improvements:
  • Perform out-of-range check during initialisation, only once for each sample
• Remove out of bound check from distance window validator. Instead, handle this by setting loop end criterion.

[JaccovdS]

@jonbinney ping

[jonbinney]

@Patrick-Lascombe or @reinzor do either of you have time to review this code? I'm not very familiar with the speckle filter.

[Patrick-Lascombe]

From what I can see the behavior is identical, but since I'm no longer using ROS1 I couldn't test it. Maybe @reinzor or someone else can test it more thoroughly.

[reinzor]

I will try to test it in our set-up today

[reinzor]

Seems to work fine in simulation; today we will perform some more tests on our hardware setup

[YannickdeHoop]

Seems to work fine with an actual hardware setup