feat(perception): terrain roughness estimator via Gabor + LBP (Issue #296) #305

Merged
sl-jetson merged 1 commits from sl-perception/issue-296-terrain-rough into main 2026-03-02 21:35:42 -05:00
4 changed files with 644 additions and 0 deletions
Showing only changes of commit 3bf603f685 - Show all commits

View File

@ -0,0 +1,219 @@
"""
_terrain_roughness.py Terrain roughness estimation via Gabor energy + LBP variance
(no ROS2 deps).
Algorithm
---------
Both features are computed on the greyscale crop of the bottom *roi_frac* of a BGR
D435i colour frame (the floor region).
1. Gabor filter-bank energy (4 orientations × 2 spatial wavelengths, quadrature pairs):
E = mean over all (θ, λ) of mean_pixels( |real_resp|² + |imag_resp|² )
A quadrature pair uses phase ψ=0 (real) and ψ=π/2 (imaginary), giving orientation-
and phase-invariant energy. The mean across all filter pairs gives a single energy
scalar that is high for textured/edgy surfaces and near-zero for smooth ones.
2. LBP variance (Local Binary Pattern, radius=1, 8-point, pure NumPy, no sklearn):
LBP(x,y) = Σ_k s(g_k g_c) · 2^k (k = 0..7, g_c = centre pixel)
Computed as vectorised slice comparisons over the 8 cardinal/diagonal neighbours.
The metric is var(LBP image). A constant surface gives LBP 0xFF (all bits set,
since every neighbour ties with the centre) variance = 0. Irregular textures
produce a dispersed LBP histogram high variance.
3. Normalised roughness blend:
roughness = clip( _W_GABOR · E / _GABOR_REF + _W_LBP · V / _LBP_REF , 0, 1 )
Calibration: _GABOR_REF is tuned so that a coarse gravel / noise image maps to ~1.0;
_LBP_REF = 5000 ( variance of a fully random 8-bit LBP image).
Public API
----------
RoughnessResult(roughness, gabor_energy, lbp_variance)
estimate_roughness(bgr, roi_frac=0.40) -> RoughnessResult
"""
from __future__ import annotations
from typing import NamedTuple, Optional
import numpy as np
# ── Tuning constants ──────────────────────────────────────────────────────────
# Gabor filter-bank parameters
_GABOR_N_ORIENTATIONS = 4 # θ ∈ {0°, 45°, 90°, 135°}
_GABOR_WAVELENGTHS = [5.0, 10.0] # spatial wavelengths in pixels (medium + coarse)
_GABOR_KSIZE = 11 # kernel side length
_GABOR_SIGMA = 3.0 # Gaussian envelope std (pixels)
_GABOR_GAMMA = 0.5 # spatial aspect ratio
# Normalization references
_GABOR_REF = 500.0 # Gabor mean power at which we consider maximum roughness
_LBP_REF = 5000.0 # LBP variance at which we consider maximum roughness
# Blend weights (must sum to 1)
_W_GABOR = 0.5
_W_LBP = 0.5
# Module-level cache for the kernel bank (built once per process)
_gabor_kernels: Optional[list] = None
# ── Data types ────────────────────────────────────────────────────────────────
class RoughnessResult(NamedTuple):
"""Terrain roughness estimate for a single frame."""
roughness: float # blended score in [0, 1]; 0=smooth, 1=rough
gabor_energy: float # raw Gabor mean energy (≥ 0)
lbp_variance: float # raw LBP image variance (≥ 0)
# ── Internal helpers ──────────────────────────────────────────────────────────
def _build_gabor_kernels() -> list:
"""Build (real_kernel, imag_kernel) pairs for all (orientation, wavelength) combos."""
import cv2
kernels = []
for i in range(_GABOR_N_ORIENTATIONS):
theta = float(i) * np.pi / _GABOR_N_ORIENTATIONS
for lam in _GABOR_WAVELENGTHS:
real_k = cv2.getGaborKernel(
(_GABOR_KSIZE, _GABOR_KSIZE),
_GABOR_SIGMA, theta, lam, _GABOR_GAMMA, 0.0,
ktype=cv2.CV_64F,
).astype(np.float32)
imag_k = cv2.getGaborKernel(
(_GABOR_KSIZE, _GABOR_KSIZE),
_GABOR_SIGMA, theta, lam, _GABOR_GAMMA, np.pi / 2.0,
ktype=cv2.CV_64F,
).astype(np.float32)
kernels.append((real_k, imag_k))
return kernels
def _get_gabor_kernels() -> list:
global _gabor_kernels
if _gabor_kernels is None:
_gabor_kernels = _build_gabor_kernels()
return _gabor_kernels
def gabor_energy(grey: np.ndarray) -> float:
"""
Mean quadrature Gabor energy across the filter bank.
Parameters
----------
grey : (H, W) uint8 or float ndarray
Returns
-------
float mean power ( 0); higher for textured surfaces
"""
import cv2
img = np.asarray(grey, dtype=np.float32)
if img.size == 0:
return 0.0
# Subtract the mean so constant regions (DC) contribute zero energy.
# This is standard in Gabor texture analysis and ensures that a flat
# uniform surface always returns energy ≈ 0.
img = img - float(img.mean())
kernels = _get_gabor_kernels()
total = 0.0
for real_k, imag_k in kernels:
r = cv2.filter2D(img, cv2.CV_32F, real_k)
i = cv2.filter2D(img, cv2.CV_32F, imag_k)
total += float(np.mean(r * r + i * i))
return total / len(kernels) if kernels else 0.0
def lbp_variance(grey: np.ndarray) -> float:
"""
Variance of the 8-point radius-1 LBP image (pure NumPy, no sklearn).
Parameters
----------
grey : (H, W) uint8 ndarray
Returns
-------
float variance of LBP values; 0 for flat surfaces, high for irregular textures
"""
g = np.asarray(grey, dtype=np.int32)
h, w = g.shape
if h < 3 or w < 3:
return 0.0
# Centre pixel patch (interior only, 1-px border excluded)
c = g[1:h-1, 1:w-1]
# 8-neighbourhood comparisons using array slices (vectorised, no loops)
lbp = (
((g[0:h-2, 0:w-2] >= c).view(np.uint8) ) |
((g[0:h-2, 1:w-1] >= c).view(np.uint8) << 1 ) |
((g[0:h-2, 2:w ] >= c).view(np.uint8) << 2 ) |
((g[1:h-1, 0:w-2] >= c).view(np.uint8) << 3 ) |
((g[1:h-1, 2:w ] >= c).view(np.uint8) << 4 ) |
((g[2:h , 0:w-2] >= c).view(np.uint8) << 5 ) |
((g[2:h , 1:w-1] >= c).view(np.uint8) << 6 ) |
((g[2:h , 2:w ] >= c).view(np.uint8) << 7 )
)
return float(lbp.var())
# ── Public API ────────────────────────────────────────────────────────────────
def estimate_roughness(
bgr: np.ndarray,
roi_frac: float = 0.40,
) -> RoughnessResult:
"""
Estimate terrain roughness from the bottom roi_frac of a BGR image.
Parameters
----------
bgr : (H, W, 3) uint8 BGR ndarray (or greyscale (H, W))
roi_frac : fraction of image height to use as the floor ROI (from bottom)
Returns
-------
RoughnessResult(roughness, gabor_energy, lbp_variance)
roughness is in [0, 1] where 0=smooth, 1=rough.
"""
import cv2
# Crop to bottom roi_frac
h = bgr.shape[0]
r0 = int(h * max(0.0, 1.0 - min(float(roi_frac), 1.0)))
roi = bgr[r0:, :]
if roi.size == 0:
return RoughnessResult(roughness=0.0, gabor_energy=0.0, lbp_variance=0.0)
# Convert to greyscale
if roi.ndim == 3:
grey = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
else:
grey = np.asarray(roi, dtype=np.uint8)
ge = gabor_energy(grey)
lv = lbp_variance(grey)
gabor_norm = min(ge / _GABOR_REF, 1.0)
lbp_norm = min(lv / _LBP_REF, 1.0)
roughness = float(np.clip(_W_GABOR * gabor_norm + _W_LBP * lbp_norm, 0.0, 1.0))
return RoughnessResult(roughness=roughness, gabor_energy=ge, lbp_variance=lv)

View File

@ -0,0 +1,102 @@
"""
terrain_rough_node.py D435i terrain roughness estimator (Issue #296).
Subscribes to the RealSense colour stream, computes Gabor filter texture
energy + LBP variance on the bottom-40% ROI (floor region), and publishes
a normalised roughness score at 2 Hz.
Intended use: speed adaptation reduce maximum velocity when roughness is high.
Subscribes (BEST_EFFORT):
/camera/color/image_raw sensor_msgs/Image BGR8
Publishes:
/saltybot/terrain_roughness std_msgs/Float32 roughness in [0,1] (0=smooth, 1=rough)
Parameters
----------
roi_frac float 0.40 Fraction of image height used as floor ROI (from bottom)
publish_hz float 2.0 Publication rate (Hz)
"""
from __future__ import annotations
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from std_msgs.msg import Float32
from ._terrain_roughness import estimate_roughness
_SENSOR_QOS = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=4,
)
class TerrainRoughNode(Node):
def __init__(self) -> None:
super().__init__('terrain_rough_node')
self.declare_parameter('roi_frac', 0.40)
self.declare_parameter('publish_hz', 2.0)
self._roi_frac = float(self.get_parameter('roi_frac').value)
publish_hz = float(self.get_parameter('publish_hz').value)
self._bridge = CvBridge()
self._latest_bgr = None # updated by subscription callback
self._sub = self.create_subscription(
Image, '/camera/color/image_raw', self._on_image, _SENSOR_QOS)
self._pub = self.create_publisher(
Float32, '/saltybot/terrain_roughness', 10)
period = 1.0 / max(publish_hz, 0.1)
self._timer = self.create_timer(period, self._on_timer)
self.get_logger().info(
f'terrain_rough_node ready — roi_frac={self._roi_frac} '
f'publish_hz={publish_hz}'
)
# ── Callbacks ─────────────────────────────────────────────────────────────
def _on_image(self, msg: Image) -> None:
try:
self._latest_bgr = self._bridge.imgmsg_to_cv2(
msg, desired_encoding='bgr8')
except Exception as exc:
self.get_logger().error(
f'cv_bridge: {exc}', throttle_duration_sec=5.0)
def _on_timer(self) -> None:
if self._latest_bgr is None:
return
result = estimate_roughness(self._latest_bgr, roi_frac=self._roi_frac)
msg = Float32()
msg.data = result.roughness
self._pub.publish(msg)
def main(args=None) -> None:
rclpy.init(args=args)
node = TerrainRoughNode()
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

View File

@ -43,6 +43,8 @@ setup(
'color_segmenter = saltybot_bringup.color_segment_node:main', 'color_segmenter = saltybot_bringup.color_segment_node:main',
# Motion blur detector (Issue #286) # Motion blur detector (Issue #286)
'blur_detector = saltybot_bringup.blur_detect_node:main', 'blur_detector = saltybot_bringup.blur_detect_node:main',
# Terrain roughness estimator (Issue #296)
'terrain_roughness = saltybot_bringup.terrain_rough_node:main',
], ],
}, },
) )

View File

@ -0,0 +1,321 @@
"""
test_terrain_roughness.py Unit tests for terrain roughness helpers (no ROS2 required).
Covers:
RoughnessResult:
- fields accessible by name
- roughness in [0, 1]
lbp_variance:
- returns float
- solid image returns 0.0
- noisy image returns positive value
- noisy > solid (strictly)
- small image (< 3×3) returns 0.0
- output is non-negative
gabor_energy:
- returns float
- solid image returns near-zero energy
- noisy image returns positive energy
- noisy > solid (strictly)
- empty image returns 0.0
- output is non-negative
estimate_roughness output contract:
- returns RoughnessResult
- roughness in [0, 1] for all test images
- roughness == 0.0 for solid (constant) image (both metrics are 0)
- roughness > 0.0 for random noise image
- roi_frac=1.0 uses entire frame
- roi_frac=0.0 returns zero roughness (empty ROI)
- gabor_energy field matches standalone gabor_energy() on the ROI
- lbp_variance field matches standalone lbp_variance() on the ROI
estimate_roughness ordering:
- random noise roughness > solid roughness (strict)
- checkerboard roughness > smooth gradient roughness
- rough texture > smooth texture
estimate_roughness ROI:
- roughness changes when roi_frac changes (textured bottom, smooth top)
- bottom-only ROI gives higher roughness than top-only on bottom-textured image
"""
import sys
import os
import numpy as np
import pytest
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
from saltybot_bringup._terrain_roughness import (
RoughnessResult,
gabor_energy,
lbp_variance,
estimate_roughness,
)
# ── Image factories ───────────────────────────────────────────────────────────
def _solid(val=128, h=64, w=64) -> np.ndarray:
return np.full((h, w, 3), val, dtype=np.uint8)
def _grey_solid(val=128, h=64, w=64) -> np.ndarray:
return np.full((h, w), val, dtype=np.uint8)
def _noise(h=64, w=64, seed=42) -> np.ndarray:
rng = np.random.default_rng(seed)
data = rng.integers(0, 256, (h, w, 3), dtype=np.uint8)
return data
def _grey_noise(h=64, w=64, seed=42) -> np.ndarray:
rng = np.random.default_rng(seed)
return rng.integers(0, 256, (h, w), dtype=np.uint8)
def _checkerboard(h=64, w=64, tile=8) -> np.ndarray:
grey = np.zeros((h, w), dtype=np.uint8)
for r in range(h):
for c in range(w):
if ((r // tile) + (c // tile)) % 2 == 0:
grey[r, c] = 255
return np.stack([grey, grey, grey], axis=-1)
def _gradient(h=64, w=64) -> np.ndarray:
"""Smooth horizontal linear gradient."""
row = np.linspace(0, 255, w, dtype=np.uint8)
grey = np.tile(row, (h, 1))
return np.stack([grey, grey, grey], axis=-1)
# ── RoughnessResult ───────────────────────────────────────────────────────────
class TestRoughnessResult:
def test_fields_accessible(self):
r = RoughnessResult(roughness=0.5, gabor_energy=100.0, lbp_variance=200.0)
assert r.roughness == pytest.approx(0.5)
assert r.gabor_energy == pytest.approx(100.0)
assert r.lbp_variance == pytest.approx(200.0)
def test_roughness_in_range(self):
for v in [0.0, 0.5, 1.0]:
r = RoughnessResult(roughness=v, gabor_energy=0.0, lbp_variance=0.0)
assert 0.0 <= r.roughness <= 1.0
# ── lbp_variance ─────────────────────────────────────────────────────────────
class TestLbpVariance:
def test_returns_float(self):
assert isinstance(lbp_variance(_grey_solid()), float)
def test_non_negative(self):
for img in [_grey_solid(), _grey_noise(), _grey_solid(0)]:
assert lbp_variance(img) >= 0.0
def test_solid_returns_zero(self):
"""Constant image: all neighbours equal centre → all LBP bits set → zero variance."""
assert lbp_variance(_grey_solid(128)) == pytest.approx(0.0)
def test_solid_black_returns_zero(self):
assert lbp_variance(_grey_solid(0)) == pytest.approx(0.0)
def test_solid_white_returns_zero(self):
assert lbp_variance(_grey_solid(255)) == pytest.approx(0.0)
def test_noise_returns_positive(self):
assert lbp_variance(_grey_noise()) > 0.0
def test_noise_greater_than_solid(self):
assert lbp_variance(_grey_noise()) > lbp_variance(_grey_solid())
def test_small_image_returns_zero(self):
"""Image smaller than 3×3 has no interior pixels."""
assert lbp_variance(np.zeros((2, 2), dtype=np.uint8)) == pytest.approx(0.0)
def test_checkerboard_high_variance(self):
"""Alternating 0/255 checkerboard → LBP alternates 0x00/0xFF → very high variance."""
v = lbp_variance(np.tile(
np.array([[0, 255], [255, 0]], dtype=np.uint8), (16, 16)))
assert v > 1000.0, f'checkerboard LBP variance too low: {v:.1f}'
def test_noise_higher_than_tiled_gradient(self):
"""Random 2D noise has varying neighbourhood patterns → higher LBP variance
than a horizontal gradient tiled into rows (which has a constant pattern)."""
assert lbp_variance(_grey_noise()) > lbp_variance(_grey_solid())
# ── gabor_energy ──────────────────────────────────────────────────────────────
class TestGaborEnergy:
def test_returns_float(self):
assert isinstance(gabor_energy(_grey_solid()), float)
def test_non_negative(self):
for img in [_grey_solid(), _grey_noise(), _grey_solid(0)]:
assert gabor_energy(img) >= 0.0
def test_solid_near_zero(self):
"""Constant image has zero Gabor response after DC subtraction."""
e = gabor_energy(_grey_solid(128))
assert e < 1.0, f'solid image gabor energy should be ~0 after DC removal, got {e:.4f}'
def test_noise_positive(self):
assert gabor_energy(_grey_noise()) > 0.0
def test_noise_greater_than_solid(self):
assert gabor_energy(_grey_noise()) > gabor_energy(_grey_solid())
def test_empty_image_returns_zero(self):
assert gabor_energy(np.zeros((0, 0), dtype=np.uint8)) == pytest.approx(0.0)
def test_noise_higher_than_solid(self):
"""Random noise has non-zero energy at our Gabor filter frequencies; solid has none."""
assert gabor_energy(_grey_noise()) > gabor_energy(_grey_solid())
# ── estimate_roughness — output contract ──────────────────────────────────────
class TestEstimateRoughnessContract:
def test_returns_roughness_result(self):
r = estimate_roughness(_solid())
assert isinstance(r, RoughnessResult)
def test_roughness_in_range_solid(self):
r = estimate_roughness(_solid())
assert 0.0 <= r.roughness <= 1.0
def test_roughness_in_range_noise(self):
r = estimate_roughness(_noise())
assert 0.0 <= r.roughness <= 1.0
def test_roughness_in_range_checkerboard(self):
r = estimate_roughness(_checkerboard())
assert 0.0 <= r.roughness <= 1.0
def test_solid_roughness_exactly_zero(self):
"""Both metrics are 0 for a constant image → roughness = 0.0 exactly."""
r = estimate_roughness(_solid(128))
assert r.roughness == pytest.approx(0.0, abs=1e-6)
def test_noise_roughness_positive(self):
r = estimate_roughness(_noise())
assert r.roughness > 0.0
def test_gabor_energy_field_nonneg(self):
assert estimate_roughness(_solid()).gabor_energy >= 0.0
assert estimate_roughness(_noise()).gabor_energy >= 0.0
def test_lbp_variance_field_nonneg(self):
assert estimate_roughness(_solid()).lbp_variance >= 0.0
assert estimate_roughness(_noise()).lbp_variance >= 0.0
def test_roi_frac_1_uses_full_frame(self):
"""roi_frac=1.0 should use the entire image."""
img = _noise(h=64, w=64)
r_full = estimate_roughness(img, roi_frac=1.0)
assert r_full.roughness > 0.0
def test_roi_frac_0_returns_zero(self):
"""roi_frac=0.0 → empty crop → all zeros."""
r = estimate_roughness(_noise(), roi_frac=0.0)
assert r.roughness == pytest.approx(0.0)
assert r.gabor_energy == pytest.approx(0.0)
assert r.lbp_variance == pytest.approx(0.0)
def test_gabor_field_matches_standalone(self):
"""gabor_energy field should match standalone gabor_energy() on the ROI."""
import cv2
img = _noise(h=64, w=64)
roi_frac = 0.5
h = img.shape[0]
r0 = int(h * (1.0 - roi_frac))
roi_grey = cv2.cvtColor(img[r0:, :], cv2.COLOR_BGR2GRAY)
expected = gabor_energy(roi_grey)
result = estimate_roughness(img, roi_frac=roi_frac)
assert result.gabor_energy == pytest.approx(expected, rel=1e-4)
def test_lbp_field_matches_standalone(self):
"""lbp_variance field should match standalone lbp_variance() on the ROI."""
import cv2
img = _noise(h=64, w=64)
roi_frac = 0.5
h = img.shape[0]
r0 = int(h * (1.0 - roi_frac))
roi_grey = cv2.cvtColor(img[r0:, :], cv2.COLOR_BGR2GRAY)
expected = lbp_variance(roi_grey)
result = estimate_roughness(img, roi_frac=roi_frac)
assert result.lbp_variance == pytest.approx(expected, rel=1e-4)
# ── estimate_roughness — ordering ────────────────────────────────────────────
class TestEstimateRoughnessOrdering:
def test_noise_rougher_than_solid(self):
r_noise = estimate_roughness(_noise())
r_solid = estimate_roughness(_solid())
assert r_noise.roughness > r_solid.roughness
def test_checkerboard_rougher_than_gradient(self):
r_cb = estimate_roughness(_checkerboard(64, 64, tile=4))
r_grad = estimate_roughness(_gradient())
assert r_cb.roughness > r_grad.roughness, (
f'checkerboard={r_cb.roughness:.3f} should exceed gradient={r_grad.roughness:.3f}')
def test_noise_roughness_exceeds_half(self):
"""For fully random noise, LBP component alone contributes ≥0.5 to roughness."""
r = estimate_roughness(_noise(h=64, w=64), roi_frac=1.0)
assert r.roughness >= 0.5, (
f'random noise roughness should be ≥0.5, got {r.roughness:.3f}')
def test_high_roi_frac_uses_more_data(self):
"""More floor area → LBP/Gabor computed on more pixels; score should be consistent."""
img = _noise(h=128, w=64)
r_small = estimate_roughness(img, roi_frac=0.10)
r_large = estimate_roughness(img, roi_frac=0.90)
# Both should be positive since the full image is noise
assert r_small.roughness >= 0.0
assert r_large.roughness >= 0.0
# ── estimate_roughness — ROI sensitivity ─────────────────────────────────────
class TestEstimateRoughnessROI:
def test_rough_bottom_smooth_top(self):
"""
Image where bottom half is noise and top half is solid.
roi_frac=0.5 picks the noisy bottom high roughness.
"""
h, w = 128, 64
img = np.zeros((h, w, 3), dtype=np.uint8)
img[:h // 2, :] = 128 # smooth top
rng = np.random.default_rng(0)
img[h // 2:, :] = rng.integers(0, 256, (h // 2, w, 3), dtype=np.uint8)
r_bottom = estimate_roughness(img, roi_frac=0.50) # noisy half
r_top = estimate_roughness(img, roi_frac=0.01) # tiny slice near top
assert r_bottom.roughness > r_top.roughness, (
f'bottom(noisy)={r_bottom.roughness:.3f} should exceed top(smooth)={r_top.roughness:.3f}')
def test_roi_frac_clipped_to_valid_range(self):
"""roi_frac > 1.0 should not crash; treated as 1.0."""
r = estimate_roughness(_noise(), roi_frac=1.5)
assert 0.0 <= r.roughness <= 1.0
if __name__ == '__main__':
pytest.main([__file__, '-v'])