Skip to content

RefineBySpeed#

Overview#

The RefineBySpeed processor refines the predicted paths of objects based on their current speed. This processor is particularly useful for improving prediction accuracy of slow-moving or stationary objects by adjusting their predicted trajectories to be more consistent with their actual motion.

Purpose#

Even if objects are moving slowly or are stationary, their predicted paths generated by the perception system might still assume higher speeds or contain unrealistic trajectory shapes.

The RefineBySpeed processor addresses this by:

  1. Speed-based filtering: Only processes objects below a configurable speed threshold
  2. Path refinement: Recalculates predicted waypoints based on the object's actual current speed
  3. Trajectory smoothing: Ensures predicted paths are physically consistent with the object's motion

Algorithm#

The processor implements the following algorithm:

ALGORITHM: RefineBySpeed

INPUT: (mutable)object, speed_threshold

BEGIN
    // Only process slow objects
    IF object.speed > speed_threshold THEN
        RETURN
    END IF

    // Modify each predicted mode
    FOR EACH mode IN object.predicted_paths DO
        // Build distance array along original path
        original_distances = [0, d1, d2, d3, ...]  // cumulative distances
        original_positions = [p0, p1, p2, p3, ...]  // original waypoints
        // Calculate new distances based on actual speed
        FOR EACH i, waypoint in ENUMERATE(waypoints:=mode.path, i:=1) DO
            new_distance = object.speed × i × time_step
            // Interpolate to find a new position along original path shape
            new_position = INTERPOLATE(original_distances, original_positions, new_distance)
            waypoints[i].position = new_position
            waypoints[i].orientation = AZIMUTH_BETWEEN(waypoints[i-1], waypoints[i])
        END FOR
    END FOR
END

Parameters#

Parameter Type Default Unit Description
speed_threshold double 1.0 m/s Speed threshold below which path refinement is applied. Objects moving faster than this threshold are not processed.
interpolation string "linear" - Interpolation method to use when finding position along original path shape. Options: "linear", "spline", "spline_by_akima"

Use Cases#

Ideal Scenarios#

  • Parking lots: Vehicles moving slowly or maneuvering
  • Traffic jams: Slow-moving or stop-and-go traffic
  • Pedestrian areas: Slow-moving pedestrians and cyclists
  • Construction zones: Reduced speed scenarios
  • Highway driving: Fast-moving objects where original predictions are likely accurate
  • Emergency vehicles: Objects that may have unpredictable acceleration patterns
  • Sports scenarios: Objects with rapid speed changes

Implementation Notes#

Performance Considerations#

  • Conditional processing: Objects above speed threshold skip all computations
  • Memory efficiency: Reuses existing waypoint vectors where possible
  • Numerical stability: Includes checks for zero time steps and minimum path lengths

Edge Case Handling#

  • Empty paths: Skips processing for paths with fewer than 2 waypoints
  • Zero time step: Skips processing when time_step ≤ 0
  • Zero path length: Skips processing when total path distance is negligible (≤ 1e-6)
  • Boundary conditions: Clamps interpolation queries to valid path bounds

Configuration Example#

It should generally be placed early in the pipeline to ensure that subsequent processors work with refined, physically consistent trajectories.

/**:
  ros__parameters:
    processors: ["refine_by_speed"]
    refine_by_speed:
      speed_threshold: 1.0 # Process objects moving slower than 1.0 m/s
      interpolation: "linear" # Interpolation method. Options: "linear", "spline", "spline_by_akima"