Investigate behavior of the beam sensor model in off-map conditions

[glpuga]

Bug description

While performing benchmarking on a number of datasets, it was noticed that the office1-7 bagfile, when replayed through both nav2 and beluga in a "beam sensor" configuration, generates these trajectories:

where it can be seen that whlie beluga is off when compared to the ground truth, while nav2 is spot on.

Map:

Some notes, from the conversation about this online:

• All three times this bagfile was benchmarked the same result was obtained.
• The same bagfile, when processed using a likelihood model displays the correct behavior for both beluga and nav2.
• The issue seems to arise because the initial pose is in an unknown region of the map. This is so because the map and the ground truth were both determined from the same input bagfile, in which the lidar is front-facing and the robot never turns to look backwards.
• When initialized in an unknown region, the initial particle cloud collapses because unknown pixels in the map are treated as obstacles by the beam sensor. Both nav2 and beluga apply the same policy in this respect; however, for reason to determine nav2 shows better resiliency and manages to provide correct poses even in this condition.

Platform (please complete the following information):

• OS: Ubuntu 22.04, ROS Humble
• Beluga version: ros-humble-beluga-amcl/jammy 2.0.2-1jammy.20240620.215909 amd64

How to reproduce

List steps to reproduce the issue:

Execute the OpenLoris/Office1-7 benchmark in the multi-dataset testbench using lambkin.

Additional context

Any other information you think could be meaningful to this issue.

[glpuga]

This is the behavior for both Beluga and Nav2 in the same dataset, but using the Likelihood model:

likelihood_both.webm

[glpuga]

This is Beluga using the beam model:

beam_beluga.webm

[glpuga]

This is Nav2 using the beam model:

beam_nav2.webm

[hidmic]

When initialized in an unknown region, the initial particle cloud collapses because unknown pixels in the map are treated as obstacles by the beam sensor. Both nav2 and beluga apply the same policy in this respect; however, for reason to determine nav2 shows better resiliency and manages to provide correct poses even in this condition.

If I had to guess, that completely arbitrary(*) choice is the problem. Clustered estimates may be helping Nav2 (as the bulk of the probability mass on motion prediction will be off-map). I would change the beam model to look for occupied cells rather than non-free cells.

(*) Not exactly arbitrary, but that rationale does not apply to Beluga.

This is Nav2 using the beam model:

Note the particle cloud in that last video is not Nav2's. Only Beluga publishes particle clouds color coded based on weights.

[glpuga]

I did some data gathering using data collected using the Robomaster sim, in three situations.

Baseline (full map):

Robot starting in an "unknown" patch in the map:

Robot crossing "unknown" patches along the way three times (one patch is crossed twice):

The trajectory is the same in all three cases, with only the localization map changing.

I'll upload the full evo results next, but the findings are that:

• as expected, likelihood model is impervious to unknown patches in the map
• Contrary to the observations that triggered this issue, both nav2_amcl and beluga_amcl fail when navigating on unknown patches. This matches the expectations based on code.
• nav2_amcl does show a bit more resiliency when being initialized on top of a small unknown patch, which is similar to the Openloris dataset scenario, but is not impervious to it.

[glpuga]

Beam model experiments:

beam_model_results.tar.gz

Likelihood model experiments:

likelihood_model_results.tar.gz

[glpuga]

I think we can remove the "bug" label from this, as everything seems to be working as expected in its design.

It does open the question of whether the OpenLoris Office dataset is useful for us, though, but that's for another forum.

[hidmic]

I think we can remove the "bug" label from this, as everything seems to be working as expected in its design.

Fair, if we take the beam model design as a given. But IMO that design doesn't make a lot of sense when you think about it. Why assume unknown space is occupied space? Why pay any attention to range measurements that land in unknown space?