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:07:30Z

+struct LikelihoodFieldProbModelParam {
+  /// Maximum distance to obstacle.
+  double max_obstacle_distance = 100.0;
+  /// Maximum range of a laser ray.
+  double max_laser_distance = 2.0;
+  /// Weight used to combine the probability of hitting an obstacle.
+  double z_hit = 0.5;
+  /// Weight used to combine the probability of random noise in perception.
+  double z_random = 0.5;
+  /// Standard deviation of a gaussian centered arounds obstacles.
+  double sigma_hit = 0.2;
+  /// Whether to enable the beam skipping heuristic.
+  bool do_beamskip = false;
+  /// Distance to the nearest obstacle below which a beam is considered to agree with the map.
+  double beam_skip_distance = 0.5;
+  /// Fraction of particles that must agree on a beam for it to be kept.
+  double beam_skip_threshold = 0.3;
+  /// If the fraction of skipped beams exceeds this value, skipping is disabled for the update.
+  double beam_skip_error_threshold = 0.9;
+};


I'm guessing we are duplicating fields in the original struct. You can instead inherit from LikelihoodFieldModelBaseParam to extend it.

glpuga · 2026-08-03T20:19:48Z

+        const auto pz = this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob);
+        // The likelihood field is monotonically decreasing in the distance to the nearest
+        // obstacle, so "distance < beam_skip_distance" is equivalent to "pz > threshold".
+        if (pz > likelihood_threshold_) {
+          ++obs_count[i];
+        }


It's a bit unfortunate that we need to use the probability as a proxy for distance, because it will not be correct. That is mainly because there exists a laser_likelihood_max_dist that sets a maximum distance to calculate the likelihood field map, and for distances larger than that we use a flat value.

That means if the if the beam_skip_distance > laser_likelihood_max_dist , we will count all rays that hit beyond laser_likelihood_max_dist as being "in".

We don't have a distance map, which would solve this, but I think that's good because having one would bring trouble:

Double the memory usage for large maps.

laser_likelihood_max_dist exists because for most real-world maps it reduces the time it takes to calculate the distance map to very small fraction because most of the area of the map is void areas you don't care the likelihood in any case.

Both issues probably have other solutions that the ones currently in use, but that would mean a deviation from the current behavior of nav2.

We can discuss it, though.

glpuga · 2026-08-03T20:21:15Z

+  double beam_skip_distance = 0.5;
+  /// Fraction of particles that must agree on a beam for it to be kept.
+  double beam_skip_threshold = 0.3;
+  /// If the fraction of skipped beams exceeds this value, skipping is disabled for the update.
+  double beam_skip_error_threshold = 0.9;


Are these the default values used in nav2? beam_skip_threshold_ at 0.9 sounds very conservative

Nevermind. I think I got the threshold value backwards.

glpuga · 2026-08-03T20:24:31Z

+        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;


Looks like we can do this transformation using Sophus as a product of poses.

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

glpuga · 2026-08-03T20:48:54Z

+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description =
+        "Fraction of skipped beams above which beam skipping is disabled for that update, preventing divergence.";


I'm not sure I can tell from this description what the threshold does. Maybe write as "If more than this fraction of beam disagree with the map, assume localization error and disable beam skipping" or something like that.

glpuga · 2026-08-03T20:50:00Z

+
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description = "Fraction of particles that must agree on a beam for it to be kept, used for beam "


Maybe "Agreement threshold above which beam-skipping considers a hit a static map hit" or something like that.

glpuga · 2026-08-03T20:50:51Z

+
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description = "Distance to the nearest obstacle below which a beam agrees with the map, used for beam "


Suggested change

descriptor.description = "Distance to the nearest obstacle below which a beam agrees with the map, used for beam "

descriptor.description = "Distance threshold for beam-skipping to consider hit a likely true static map hit"

glpuga · 2026-08-03T20:52:10Z

    update_min_d: 0.25
    # Laser sensor model type.
-    laser_model_type: likelihood_field
+    laser_model_type: likelihood_field_prob


This is not the default value, you probably don't want to merge this.

glpuga · 2026-08-03T20:52:41Z

+    # Whether to enable beam skipping (likelihood_field_prob only). When enabled, beams that
+    # disagree with the map across most particles (e.g. unmapped or dynamic obstacles) are
+    # ignored. Disabled by default; only beneficial in scenes with unmapped/dynamic obstacles.
+    do_beamskip: true
+    # Distance to the nearest obstacle below which a beam is considered to agree with the map.
+    beam_skip_distance: 0.5
+    # Fraction of particles that must agree on a beam for it to be kept.
+    beam_skip_threshold: 0.3
+    # Fraction of skipped beams above which beam skipping is disabled for that update.
+    beam_skip_error_threshold: 0.9


Reconsider the comments with my comments on the parameters descriptors above.

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.

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

		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;

	descriptor.description = "Distance to the nearest obstacle below which a beam agrees with the map, used for beam "
	descriptor.description = "Distance threshold for beam-skipping to consider hit a likely true static map hit"

Uh oh!

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!

Choose a reason for hiding this comment

Uh oh!

glpuga Aug 3, 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!

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!

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!

glpuga commented Aug 3, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

glpuga commented Aug 3, 2026 •

edited

Loading

glpuga Aug 3, 2026 •

edited

Loading