Extending likelihood field to model unexplored spaces

[DPR00]

Proposed changes

The following changes are proposed in order to enhance the Likelihood Field Model based on the suggestions described on #55:

• First, a new function unknown_obstacle_data() is created to ideally replace the obstacle_data() function in beluga/sensor/data/OccupancyGrid.hpp. This new function will return a srd::tuple<bool, bool> containing the values which are occupied and unknown.
• Then, this new function will be passed to calculate the distance map using a new function nearest_obstacle_unknown_distance_map. This new function will ideally replace the nearest_obstacle_distance_map function.
• The value of the distance map for unknown values is assigned with a pre-computed value inside the nearest_obstacle_unknown_distance_map function. Based on this computed value, the likelihood will be $\frac{1}{z_{max}}$.

Some comments:

• It passed the compilation steps and the beluga example was tested. It would be nice to make some maps testing the draft. How could I perform that?

Type of change

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

💥 Breaking change! Explain why a non-backwards compatible change is necessary or remove this line entirely if not applicable.

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 commits have been signed for DCO

Additional comments

[hidmic]

First pass!

It would be nice to make some maps testing the draft. How could I perform that?

I recommend extending the likelihood field model unit tests for this next.

[DPR00]

Hi @hidmic! I'm trying to replicate locally the 'build_and_test.sh' file, since it is failing in the pipeline. I wanted to tested first before updating the draft PR. However, I'm getting the following error (similar to #422, but it actually build when using -colcon build --packages-up-to beluga_example --cmake-args -DBUILD_TESTING=OFF as indicated in Documentation):

-- Configuring incomplete, errors occurred!
---
--- stderr: beluga
CMake Error at test/CMakeLists.txt:30 (find_package):
  By not providing "Findbenchmark.cmake" in CMAKE_MODULE_PATH this project
  has asked CMake to find a package configuration file provided by
  "benchmark", but CMake did not find one.

  Could not find a package configuration file provided by "benchmark" with
  any of the following names:

    benchmarkConfig.cmake
    benchmark-config.cmake

  Add the installation prefix of "benchmark" to CMAKE_PREFIX_PATH or set
  "benchmark_DIR" to a directory containing one of the above files.  If
  "benchmark" provides a separate development package or SDK, be sure it has
  been installed.
---

This is happening in jazzy distro, but it actually works on humble. I also tried running it from the main branch, but it keeps throwing that error. Should I arise an issue?

[hidmic]

codecov isn't happy because we are lacking tests :)

[hidmic]

@DPR00 we should update the PR title.

[hidmic]

This is amazing @DPR00, great job!

We should add tests now though, for the likelihood model and for the action we just added.

[hidmic]

This is no draft anymore, it's solid PR material.

[DPR00]

Awesome @hidmic, thanks! I will be addressing the comments during the following days and adding tests, as well.
I also want to test it in a simulated world. Any recommendations of a world different than turtlebot_world?

[hidmic]

Third pass I think. Really good work @DPR00.

[hidmic]

This PR needs a rebase after #433.

[DPR00]

I did the rebase and reduce all the changes to one commit. The latter was due there were a lot of commits where the changes are no longer valid.

[hidmic]

I left a few more comments and CI is not happy, but this is a great piece of work @DPR00.

I'm happy with it once the missing bits are addressed and CI goes green (looks like clang-tidy is not happy).

[hidmic]

Plain amazing. I'm good with this PR as-is. @nahueespinosa ?

[nahueespinosa]

LGTM! After fixing forwarding references.

[nahueespinosa]

LGTM!

[hidmic]

@DPR00 go go go 🚀

[DPR00]

I am not able to merge it because I need to be an authorized user. Could you merge it @hidmic, pls? 🚀

[glpuga-eka]

@DPR00 @hidmic I'm going through this code to help rodrigo with #577, but I can't fully make sense of this.

After a while of thinking this go a long way around assigning 1 / params.max_laser_distance likelihood to any unknown cell. The latest version of this blurs this even more because that value may be overriden here, which may be right or may be wrong depending what we model here.

So my question is: why 1 / params.max_laser_distance? That models a uniform distribution over the lenght of the lidar assuming with 100% certainty that this is a random measurement in that range. Why is this not offset = params.z_random / params.max_laser_distance which is the overall probability of a random measurement falling here?

[DPR00]

IIRC this was took from this part of Probabilistic Robotics book (Section 6.4.2, page 173):

[hidmic]

That. IIUC the idea is that the probability of a hit for any given measurement is uniform: $z \in [0, z_{max}]$, then $p(z) = \frac{1}{z_{max}}$). Mixing parameters are not applicable because those presume knowledge we don't have (well, kinda, we could still model failure and unexpected objects, I think, but they didn't and we don't).

[glpuga-eka]

I don't think that's right, and I explain why.

The problem is that that causes the likelihood map to implement different likelihood functions depending on the lidar happens to hit, causing non-uniform weight scales. Depending on the lidar range the likelihood might be higher through an open window than 20cm away from an actually mapped obstacle.

Mixing parameters are not applicable because those presume knowledge we don't have

I think that not mixing assumes a knowledge we don't have. It asummes we know with 100% certainty that the measurement IS a random hit, which we can't know. Where a hit falls does not depend as much on the measurement as much as on the hyphotesis/particle you are tracing it from, so a hit on unknown area behind a wall surface or through a window most likely just means a hyphotesis which is 5 cm closer to the wall than the actual pose of the lidar. That hypothesis is not wrong, it's just part of the internal representation of the distribution that the filter uses for the pose.

If we assign 1 / params.max_laser_distance, we are overweighing particles with beams that hit within unknown areas, because z_random < 1.0,

If we assign a weight params.z_random / params.max_laser_distance to that case, that's the weight we assign to random impacts everywhere, and indeed that's how we model "random obstacles that are not mapped", which makes sense for unknown areas.

Notice that while I think that using params.z_random / params.max_laser_distance is at least in line with the larger model and does not distort the likelihood distribution value scale, I still don't think it is right.

That's because the likelihood model only really models known obstacles; more than a probabilistic model, LF is a "scan correlator". In fact, the z_random term is not really there to decide on the "goodness" of an impact over another, since it's the same everywhere; it's just there to avoid the likelihood product going to zero too fast if we hit unmapped obstacles.

In ignoring the z_hit term behind a wall we are biasing the estimation solution towards the external side of the wall by truncating the natural symmetry of the distance-likelihood distribution on both sides of the surface. In closed quarters this may not make much of a difference, but if you only see a straight wall on your left, this alone may add you a few centimeters of error due to that bias this causes in the particle distribution.

[hidmic]

If we assign a weight params.z_random / params.max_laser_distance to that case, that's the weight we assign to random impacts everywhere, and indeed that's how we model "random obstacles that are not mapped", which makes sense for unknown areas.

Hmm, but random hit != unknown hit. Random hits have lower probability because it is implied they are transient. Thrun mentions cross-talk and echoes as potential sources.

That said...

In ignoring the z_hit term behind a wall we are biasing the estimation solution towards the external side of the wall by truncating the natural symmetry of the distance-likelihood distribution on both sides of the surface.

Yeah, I see the problem now 🤔

(stream of consciousness ahead)

IIUC we are folding both unobservable and unmapped as unknown, and we are overpowering the base likelihood model when we shouldn't. Assigning a higher likelihood to hits within internal walls or past external walls just gives us bias. Assigning a higher likelihood to hits within an unmapped but otherwise visible region does prevent us from excessively penalizing hypothesis close to such regions when those regions happen to be occupied. If the unmapped space is within a room that had its door closed during mapping, we do want hits within that unexplored space not be ruled out as highly unlikely.

Hmm, what if we let the user configure a distance threshold? Unexplored space that is within that distance from occupied cells decays normally, while unexplored space past that distance gets the truncated likelihood to signal we don't really know if a hit in that region is likely or not. We cover for the variance in the estimate while not (excessively) penalizing hitting a car parked out the window.