Ground Filtering (GNC)

Grid

prod/26x: 32 segments × 10 range bins. feature/perc/lidar-dev: about 20 × 25. Segment width is $180^\circ / N_{\text{segments}}$.

Cone returns sit a few tens of centimetres above the track. A fixed world-$z$ threshold fails when the car pitches or rolls, or when ground height changes with range. Grace & Conrad (GnC) estimates a local ground line in each angular wedge from the LiDAR data alone (no IMU).

Model

Points for one completed angular segment live in a range-binned buffer. For that wedge:

1. In each non-empty range bin, take the minimum-$z$ return as a ground sample along that ray.
2. Fit least squares of those $z_{\min}$ values versus horizontal radius $r$. Slope and intercept are the local ground line.
3. Keep returns whose height $z$ lies in a band above that line:

$$\begin{aligned} z_{\text{low}}(r) &= \text{slope}\cdot r + \text{intercept} + h_{\text{low}} \\ z_{\text{high}}(r) &= \text{slope}\cdot r + \text{intercept} + h_{\text{high}} \end{aligned}$$

with $h_{\text{low}} =$ height_threshold, $h_{\text{high}} =$ upper_height_threshold (mm inside the node; YAML is metres).

Below the band is treated as ground; above as non-cone structure (walls, people, noise). Filtering lags by two segments so an active wedge is not cut mid-fill. Remaining segments flush before DBSCAN at publish time.

Parameters

Parameter	Typical default	Notes
height_threshold	100 mm	Floor of the keep-band above the fitted line
upper_height_threshold	500 mm	Ceiling; should clear a cone (~325 mm)
$N_{\text{segments}}$, $N_{\text{bins}}$	compile-time	How local the ground fit is

Defaults live in driver params and compile-time grid constants.

Failure modes

Symptom	Likely cause
/lidar/ground_filtered looks like raw ground	Band too wide or fit failing (empty bins)
Cones missing at close range, far range ok	height_threshold can clip cone bases nearby (FIXME in code)
Structure kept, cones gone	upper_height_threshold too low, or cutoffs upstream

stream_processor.cpp::filter_out_ground. Related historical path: pcl_pipeline_cpp GraceAndConrad.

Next stage: DBSCAN. Driver overview: LiDAR driver.