Added: beam-skipping to likelihood-field-prob by glpuga · Pull Request #577 · Ekumen-OS/beluga

glpuga · 2026-08-03T20:04:05Z

Proposed changes

Draft of the beam skipping feature, companion to the likelihood_field_prob sensor model .

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

Anything worth mentioning to the reviewers.

glpuga

It's a great first pass @ralcoberro

glpuga · 2026-08-03T20:28:37Z

+      double log_weight = 0.0;
+      for (std::size_t i = 0; i < points.size(); ++i) {
+        // Skip beams that were masked out by prepare(). When beam skipping is disabled (or
+        // prepare() was never called) the mask is not consulted and every beam contributes,
+        // reproducing the plain likelihood field prob behavior.
+        if (do_beamskip_ && i < beam_mask_.size() && !beam_mask_[i]) {
+          continue;
+        }
+        // Transform the end point of the laser to the grid local coordinate system.
+        // Not using Eigen/Sophus because they make the routine x10 slower.
+        // See `benchmark_likelihood_field_model.cpp` for reference.
+        const auto& point = points[i];
+        const auto x = point.first * cos_theta - point.second * sin_theta + x_offset;
+        const auto y = point.first * sin_theta + point.second * cos_theta + y_offset;
+        const auto pz =
+            static_cast<double>(this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob));
+        log_weight += std::log(pz);
+      }
+      return std::exp(log_weight);


You can do this without resorting back to for loops and at the same time reduce the amount of changes by zipping together the beam_mask and the points vectors, and returning 0.0 from the lambda if the beam is masked. You don't need to check do_beamskip_ because the mask will be true for all beams in that case anyways.,

For an example see https://github.com/Ekumen-OS/beluga/blob/main/beluga/include/beluga/sensor/likelihood_field_model_base.hpp#L165-L175

Done. operator() is back to a declarative accumulation: it zips points with the beam mask and the projection returns 0.0 (the neutral element of the log-likelihood sum) for masked beams, so there is no do_beamskip_ check and no index bookkeeping in the hot path.
To make that work I had prepare() always size the mask to the measurement, leaving every beam enabled when skipping is disabled, which is the invariant your comment relies on. operator() also resolves the mask once per update before building the lambda: if prepare() was not called for this measurement (the model used standalone, as in the unit tests) it falls back to an all-enabled mask, so the plain likelihood-field-prob behavior is preserved. That resolution is once per update, not per particle. Covered by a new WithoutPrepareIntegratesEveryBeam test.

glpuga · 2026-08-03T20:57:05Z

@griswaldbrooks , this is the WIP beam-skipping feature that @ralcoberro has been working on that we mentioned last week.

…perator())

glpuga · 2026-10-06T15:33:08Z

    const std::size_t num_beams = points.size();
-    beam_mask_.assign(num_beams, true);
-    if (num_beams == 0) {
+    beam_mask_.assign(num_beams, std::uint8_t{1});


Use a static cast instead.

glpuga · 2026-10-06T15:40:14Z

+        // Transform the end point of the laser to the grid local coordinate system.
+        // Not using Eigen/Sophus because they make the routine x10 slower.
+        // See `benchmark_likelihood_field_model.cpp` for reference.


As a general statement this is dubious to me, since for SE2 Sophus literally store represents the orientation as the sin and cosin values, so the product itself cannot really be calculated that much different from this. If it takes 10x its because we are loading the sophus data and doing an arctan/log at some point along the chain.

Still, not for this PR to fix, but I would remove the comment not to repeat an very broad assertion without contextual verifcation.

glpuga · 2026-10-06T15:40:35Z

    const double skipped_ratio = static_cast<double>(skipped) / static_cast<double>(num_beams);
    if (skipped_ratio > beam_skip_error_threshold_) {
-      std::fill(beam_mask_.begin(), beam_mask_.end(), true);
+      std::fill(beam_mask_.begin(), beam_mask_.end(), std::uint8_t{1});


use static cast.

glpuga · 2026-10-06T15:43:28Z

+    // plain likelihood field prob behavior.
+    auto mask = beam_mask_.size() == points.size()  //
+                    ? beam_mask_
+                    : std::vector<std::uint8_t>(points.size(), std::uint8_t{1});


This will materialize an std::vector, allocating memory on each call, substantially slowing down the function. Use a ranges generator that returns the same number on all invocations.

glpuga · 2026-10-06T15:49:01Z

+    // further bookkeeping. When prepare() was not called for this measurement (e.g. when the model
+    // is used standalone, outside the two pass update) every beam is integrated, reproducing the
+    // plain likelihood field prob behavior.
+    auto mask = beam_mask_.size() == points.size()  //


doesn't prepare ensure that these two are equal length?

glpuga · 2026-10-06T15:51:28Z

    const std::size_t num_beams = points.size();
-    beam_mask_.assign(num_beams, true);
-    if (num_beams == 0) {
+    beam_mask_.assign(num_beams, std::uint8_t{1});


Clear the vector and reserve() instead of using asssing. assign will spend time O(N) setting the whole vector to 1s, only for these to be overwritten in line 164.

Better leave it empty here, reseve the storage to avoid reallocations, and push_back() the values in line 164.

In line 127, do std::fill to ensure these are all 1s before returning if we are not doing beam_skipping.

glpuga · 2026-10-06T16:01:44Z

+  double beam_skip_threshold_;           ///< Fraction of particles that must agree for a beam to be kept.
+  double beam_skip_error_threshold_;     ///< Skipped-beam fraction above which skipping is disabled.
+  float likelihood_threshold_;           ///< Likelihood equivalent of `beam_skip_distance`.
+  std::vector<std::uint8_t> beam_mask_;  ///< Per-beam mask computed by `prepare()` (non-zero means used).


A vector of booleans would work here.

I would consider keeping the std::vector<std::uint8_t> in case you want to parallelize.

prepare is now the only pass in Amcl::update that doesn't take the execution policy, even though it does the same particles × beams work as reweight. A way to parallelize it is per beam, with each worker writing mask[i]. The standard explicitly excludes std::vector<bool> from the guarantee that distinct elements can be modified concurrently ([container.reqmts], data races), because neighboring bits share a word. So with std::vector<bool>, passing policy to prepare later would quietly introduce a data race. With uint8_t it's safe as is.

glpuga · 2026-10-06T16:07:48Z

Please rebase on top of the current main and check that the tests pass.

Added: beam-skipping to likelihood-field-prob

0cce34c

glpuga commented Aug 3, 2026

View reviewed changes

glpuga requested a review from LaBruma August 3, 2026 20:54

glpuga assigned ralcoberro Aug 3, 2026

hidmic self-requested a review August 4, 2026 14:26

Modif: Address PR's comments

d9528ab

glpuga-eka mentioned this pull request Sep 2, 2026

Extending likelihood field to model unexplored spaces #430

Merged

5 of 7 tasks

Modif: Address PR's comments (beam skip distance constraint, zipped o…

d2711f5

…perator())

glpuga commented Oct 6, 2026

View reviewed changes

Conversation

glpuga commented Aug 3, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Proposed changes

Type of change

Checklist

Additional comments

Uh oh!

glpuga left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

glpuga commented Aug 3, 2026

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

griswaldbrooks Oct 7, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

glpuga commented Oct 6, 2026

Uh oh!

This branch has not been deployed

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

glpuga commented Aug 3, 2026 •

edited

Loading

griswaldbrooks Oct 7, 2026 •

edited

Loading