devkit_ui.dem

dem.py ────── Builds a DEM (elevation grid) from sparse recon points — the CSV logged by devkit_driver/modules/recon_dem_logger.py — then derives a traversability mask from it. This is the missing link between recon logging and terrain_mask.py’s ring vectorizer (see repo issue #110):

recon.csv –[this module]–> elevation grid –[this module]–> traversable

mask –[terrain_mask.py]–> obstacle_rings –[f2c_planner]–> _run_f2c()

Interpolation backend: scipy.interpolate.RBFInterpolator. RTK recon points are metres apart, not point-cloud density, so a thin-plate spline is enough — no need for grid_map_pcl or a C++ dependency. _interpolate_elevation() is the single seam to swap in verde.Spline later if RBF proves too crude on real field data; the points-in/grid-out shape is the same for both, so that swap should be a one-function change, not a rewrite. (Measured against verde.Spline on synthetic noisy data: comparable accuracy at matched regularization — the RBF-vs-Spline choice isn’t the thing that matters here, see below.)

_choose_smoothing()’s cross-validation loop is adapted from verde.SplineCV’s approach — grid-search a set of regularization candidates, score each against held-out folds, keep the lowest-error one — credit: fatiando/verde (github.com/fatiando/verde), BSD-3-Clause License, Copyright (c) 2017 The Verde Developers. Reimplemented directly on top of RBFInterpolator instead of depending on verde.SplineCV, because Verde’s only runtime dependency this module would actually use is that CV loop — import verde unconditionally pulls in pandas, xarray, scikit-learn, dask, and pooch (measured: ~2.4s import time) for a package meant to run on an embedded SBC (Avaota A1). Not worth it for one grid-search loop.

elevation_to_traversable() mirrors the slope math in devkit_bringup/config/terrain_traversability_filters.yaml’s filter3/filter4 (acos(normal_z) threshold) so behaviour matches if that grid_map_filters chain is ever wired in instead of this.

Pure functions, no ROS dependency — callers read the CSV themselves and pass plain numpy arrays, same convention as terrain_mask.py.

Functions

build_elevation_grid(xy, elevation, resolution_m)	Interpolate scattered recon points onto a regular elevation grid.
elevation_to_traversable(elevation_grid, ...)	Slope -> binary traversable mask (1 = safe, 0 = unsafe), matching terrain_mask.py's expected convention.
load_recon_points(csv_path)	Read recon_dem_logger.py's CSV.
recon_csv_to_obstacle_rings(csv_path, ...[, ...])	End to end: recon CSV -> obstacle_rings ready for f2c_planner._run_f2c().
select_reference_contour_latlon(...[, ...])	select_reference_contour_xy(), reprojected to lat/lon.
select_reference_contour_xy(elevation_grid, ...)	Pick and return the elevation isoline through centroid_xy.

devkit_ui.dem.build_elevation_grid(xy: ndarray, elevation: ndarray, resolution_m: float, padding_m: float = 2.0, smoothing: float | str = 'auto') → tuple[ndarray, tuple[float, float], float]

Interpolate scattered recon points onto a regular elevation grid.

Parameters:

• xy – (N, 2) local x,y metres of recon points (see load_recon_points).

• elevation – (N,) altitude, metres.

• resolution_m – metres per cell — should match whatever terrain_traversability_filters.yaml / the traversable-mask consumer expects.

• padding_m – extend the grid this far past the recon points’ bounding box in each direction. RBFInterpolator will happily extrapolate right to the field edge, but extrapolation quality degrades fast past the convex hull of the input points — pad a little, don’t rely on it for the whole field boundary.

• smoothing – RBFInterpolator’s smoothing parameter, or ‘auto’ (the default) to pick it via _choose_smoothing()’s cross-validation. Pass an explicit float to skip CV (e.g. for reproducible tests, or once you have a field-validated value you trust more).

Returns:

2D array, elevation_grid[0, 0] is the grid’s

southwest-most cell (row = y, col = x, matching terrain_mask.py’s (row, col) -> (x, y) convention).

origin_xy: (x, y) of elevation_grid[0, 0], in the same local-XY frame

as xy — pass this straight through to traversability_mask_to_latlon_rings()’s origin_xy argument.

smoothing_used: the smoothing value actually applied — inspect this

when smoothing=’auto’ to see what CV picked; log it, don’t discard it, if this ever misbehaves in the field.

Return type:

elevation_grid

devkit_ui.dem.elevation_to_traversable(elevation_grid: ndarray, resolution_m: float, max_slope_deg: float = 15.0) → ndarray

Slope -> binary traversable mask (1 = safe, 0 = unsafe), matching terrain_mask.py’s expected convention.

Same acos(normal_z)*180/pi formula as terrain_traversability_filters.yaml’s filter3/filter4, but normal_z here comes from a finite-difference gradient (np.gradient over adjacent cells), not that filter chain’s NormalVectorsFilter (least-squares plane fit over a 0.2m radius neighbourhood). The two will diverge on anything but a locally planar surface — don’t assume swapping one for the other is a no-op without checking against real elevation data. max_slope_deg is still the placeholder from that yaml — untuned against real field slope data (issue #110), unchanged here.

devkit_ui.dem.load_recon_points(csv_path: str | Path) → tuple[ndarray, ndarray, ndarray]

Read recon_dem_logger.py’s CSV.

Returns:

(N, 2) array of local x,y metres (the same columns

recon_dem_logger.py writes from /odom — no re-projection here). This is recon_dem_logger.py’s own /odom-anchored frame, NOT necessarily anchored at any particular lat/lon — callers that need xy in a frame anchored at a specific lat/lon (e.g. corners_ll[0], to match f2c_planner’s contract that origin_xy and corners_ll[0] share an anchor) must re-project using latlon below, not use this xy directly.

elevation: (N,) array of altitude, metres. latlon: (N, 2) array of lat,lon — the real-world position of each

xy point, for re-anchoring into a different local frame.

Return type:

xy

Raises:

ValueError – if the CSV has fewer than 3 points — RBFInterpolator needs at least that many to fit a surface, and 3 points can’t usefully be flagged as a bad fit for slope, so callers should treat this as “recon drive too short, log more points”.

devkit_ui.dem.recon_csv_to_obstacle_rings(csv_path: str | Path, resolution_m: float, anchor_lat: float, anchor_lon: float, max_slope_deg: float = 15.0) → list[list[tuple[float, float]]]

End to end: recon CSV -> obstacle_rings ready for f2c_planner._run_f2c().

anchor_lat/anchor_lon MUST be the same anchor _run_f2c() will use for corners_ll[0]. Recon points are re-anchored onto anchor_lat/lon (via their own lat/lon columns) before the elevation grid is built, so origin_xy ends up in the same frame corners_ll[0] projects to, regardless of whatever frame recon_dem_logger.py originally logged xy in — see traversability_mask_to_latlon_rings()’s warning on mismatched anchors silently misaligning the rings against the field.

devkit_ui.dem.select_reference_contour_latlon(elevation_grid: ndarray, resolution_m: float, origin_xy: tuple[float, float], centroid_xy: tuple[float, float], anchor_lat: float, anchor_lon: float, simplify_tolerance_m: float | None = None, min_length_m: float = 1.0) → list[tuple[float, float]] | None

select_reference_contour_xy(), reprojected to lat/lon.

anchor_lat/anchor_lon MUST be the same anchor _run_contour_f2c() will use for corners_ll[0] — same warning as traversability_mask_to_latlon_rings(): mismatched anchors silently misalign the reference line against the field boundary.

devkit_ui.dem.select_reference_contour_xy(elevation_grid: ndarray, resolution_m: float, origin_xy: tuple[float, float], centroid_xy: tuple[float, float], simplify_tolerance_m: float | None = None, min_length_m: float = 1.0) → list[tuple[float, float]] | None

Pick and return the elevation isoline through centroid_xy.

Parameters:

• elevation_grid – as returned by build_elevation_grid() — elevation_grid[row, col] sits at (origin_xy[0] + col*resolution_m, origin_xy[1] + row*resolution_m), matching terrain_mask.py’s (row=y, col=x) convention.

• resolution_m – as returned by build_elevation_grid() — elevation_grid[row, col] sits at (origin_xy[0] + col*resolution_m, origin_xy[1] + row*resolution_m), matching terrain_mask.py’s (row=y, col=x) convention.

• origin_xy – as returned by build_elevation_grid() — elevation_grid[row, col] sits at (origin_xy[0] + col*resolution_m, origin_xy[1] + row*resolution_m), matching terrain_mask.py’s (row=y, col=x) convention.

• centroid_xy – (x, y) point to centre the reference line on — pass the field boundary polygon’s centroid, in the same local-xy frame as origin_xy.

• simplify_tolerance_m – Douglas-Peucker tolerance applied to the traced contour before returning it. RBF-interpolated DEMs from sparse recon points are noisy at row-spacing wavelength; skipping this lets that noise get amplified into loops when f2c_planner offsets the line. Defaults to 1.5 * resolution_m when None — untuned against real field data, same caveat as elevation_to_traversable()’s max_slope_deg.

• min_length_m – discard contour components shorter than this — noise specks, not a usable reference line.

Returns:

List of (x, y) points tracing the isoline nearest centroid_xy, or None if no usable contour exists at the centroid’s elevation (e.g. a field flat enough that find_contours returns nothing, or the centroid falling outside the interpolated grid). Callers should treat None as “this field doesn’t need contour rows, fall back to _run_f2c()’s straight swaths” rather than an error.