When using large maps, the likelihood map constructions takes very long time

[glpuga]

Bug description

Upon map reception, building the likelihood map when a new map is received takes a long time if the map is large (ej. 200mx600m). This is because of the construction of the distance map and then likelihood map in this function.

On a Intel Core i9-9900 CPU this is around 22 seconds for a 200mx600m at 0.05 m/pixel resolution.

What's worse, during this time the node is unresponsive.
There's a related failure mode that is triggered by this, see #469 .

Platform (please complete the following information):

• OS: Any.
• Beluga version: 2.0.2

How to reproduce

Run any system using beluga and a large map. 200mx600m at 0.05 m/pixel resolution is large enough.

Expected behavior

The node should not go unresponsive for so long.

Actual behavior

Node freezes, lifecycle node complains and actually fails.

Additional context