From c5f3a5b2ce4b3e6b36faa45fa982839d98cdf249 Mon Sep 17 00:00:00 2001 From: sl-perception Date: Mon, 2 Mar 2026 20:46:35 -0500 Subject: [PATCH] =?UTF-8?q?feat(perception):=20motion=20blur=20detector=20?= =?UTF-8?q?via=20Laplacian=20variance=20=E2=80=94=20Issue=20#286?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add _blur_detector.py: BlurResult NamedTuple, laplacian_variance() (ksize=3 Laplacian on greyscale, with optional ROI crop), detect_blur() returning variance + is_blurred flag + threshold; handles greyscale and BGR inputs, empty ROI returns 0.0 - Add blur_detect_node.py: subscribes /camera/color/image_raw (BEST_EFFORT), publishes Bool /saltybot/image_blurred and Float32 /saltybot/blur_score per frame; threshold and roi_frac ROS params - Register blur_detector console script in setup.py - 25/25 unit tests pass (no ROS2 required) Co-Authored-By: Claude Sonnet 4.6 --- .../saltybot_bringup/_blur_detector.py | 105 +++++++ .../saltybot_bringup/blur_detect_node.py | 105 +++++++ jetson/ros2_ws/src/saltybot_bringup/setup.py | 2 + .../test/test_blur_detector.py | 257 ++++++++++++++++++ 4 files changed, 469 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_blur_detector.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/blur_detect_node.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/test/test_blur_detector.py diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_blur_detector.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_blur_detector.py new file mode 100644 index 0000000..69a1695 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_blur_detector.py @@ -0,0 +1,105 @@ +""" +_blur_detector.py — Motion blur detection via Laplacian variance (no ROS2 deps). + +Algorithm +--------- +1. Convert the BGR image to greyscale. +2. Apply the discrete Laplacian operator (cv2.Laplacian, ksize=3). +3. Compute the variance of the Laplacian response across the entire image. +4. A sharp image has high-contrast edges → high variance. + A blurred image smears edges → low variance. +5. Compare the variance against a configurable threshold: + blurred = (variance < threshold) + +The result is returned as a BlurResult NamedTuple. All image-free helpers +(is_blurred, etc.) work on pre-computed variance values so callers can cache +expensive computation. + +Optional ROI +------------ +If roi (row_start, row_end, col_start, col_end) is given, only that crop is +analysed. Useful for ignoring the sky or far-field clutter that would +artificially inflate variance. + +Public API +---------- + BlurResult(variance, is_blurred, threshold) + laplacian_variance(bgr, roi=None) -> float + detect_blur(bgr, threshold=100.0, roi=None) -> BlurResult +""" + +from __future__ import annotations + +from typing import NamedTuple, Optional, Tuple + + +# ── Data types ──────────────────────────────────────────────────────────────── + +class BlurResult(NamedTuple): + """Result of a single blur detection evaluation.""" + variance: float # Laplacian variance of the (cropped) image + is_blurred: bool # True when variance < threshold + threshold: float # threshold used for this evaluation + + +# ── Public API ──────────────────────────────────────────────────────────────── + +def laplacian_variance( + bgr: 'np.ndarray', + roi: Optional[Tuple[int, int, int, int]] = None, +) -> float: + """ + Compute the variance of the Laplacian of a BGR image. + + Parameters + ---------- + bgr : (H, W, 3) or (H, W) uint8 ndarray + roi : optional (row_start, row_end, col_start, col_end) — analyse this crop only + + Returns + ------- + float — Laplacian variance (≥ 0); higher = sharper + """ + import cv2 + import numpy as np + + if bgr.ndim == 3: + grey = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY) + else: + grey = np.asarray(bgr, dtype=np.uint8) + + if roi is not None: + r0, r1, c0, c1 = roi + grey = grey[r0:r1, c0:c1] + + if grey.size == 0: + return 0.0 + + lap = cv2.Laplacian(grey, cv2.CV_64F, ksize=3) + return float(lap.var()) + + +def detect_blur( + bgr: 'np.ndarray', + threshold: float = 100.0, + roi: Optional[Tuple[int, int, int, int]] = None, +) -> BlurResult: + """ + Detect whether an image is motion-blurred. + + Parameters + ---------- + bgr : (H, W, 3) uint8 BGR ndarray (or greyscale (H, W)) + threshold : Laplacian variance below which the image is considered blurred + roi : optional (row_start, row_end, col_start, col_end) crop + + Returns + ------- + BlurResult(variance, is_blurred, threshold) + """ + var = laplacian_variance(bgr, roi=roi) + return BlurResult( + variance=var, + is_blurred=var < threshold, + threshold=threshold, + ) diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/blur_detect_node.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/blur_detect_node.py new file mode 100644 index 0000000..dda9aca --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/blur_detect_node.py @@ -0,0 +1,105 @@ +""" +blur_detect_node.py — D435i motion blur detector (Issue #286). + +Subscribes to the RealSense colour stream, evaluates the Laplacian variance +blur metric, and publishes a Bool flag plus a Float32 score. + +Intended use: pause or de-weight SLAM keyframe insertion when the robot is +moving fast and the image is visibly blurred. + +Subscribes (BEST_EFFORT): + /camera/color/image_raw sensor_msgs/Image BGR8 + +Publishes: + /saltybot/image_blurred std_msgs/Bool True = blurred + /saltybot/blur_score std_msgs/Float32 Laplacian variance (higher = sharper) + +Parameters +---------- +threshold float 100.0 Variance below which image is considered blurred +roi_frac float 0.0 Fraction of image height to skip from top (0 = full frame) +""" + +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 Bool, Float32 + +from ._blur_detector import detect_blur + + +_SENSOR_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=4, +) + + +class BlurDetectNode(Node): + + def __init__(self) -> None: + super().__init__('blur_detect_node') + + self.declare_parameter('threshold', 100.0) + self.declare_parameter('roi_frac', 0.0) + + self._threshold = float(self.get_parameter('threshold').value) + self._roi_frac = float(self.get_parameter('roi_frac').value) + + self._bridge = CvBridge() + + self._sub = self.create_subscription( + Image, '/camera/color/image_raw', self._on_image, _SENSOR_QOS) + self._pub_flag = self.create_publisher(Bool, '/saltybot/image_blurred', 10) + self._pub_score = self.create_publisher(Float32, '/saltybot/blur_score', 10) + + self.get_logger().info( + f'blur_detect_node ready — threshold={self._threshold} ' + f'roi_frac={self._roi_frac}' + ) + + # ── Callback ────────────────────────────────────────────────────────────── + + def _on_image(self, msg: Image) -> None: + try: + 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) + return + + roi = None + if self._roi_frac > 0.0: + h = bgr.shape[0] + r0 = int(h * min(self._roi_frac, 0.9)) + roi = (r0, h, 0, bgr.shape[1]) + + result = detect_blur(bgr, threshold=self._threshold, roi=roi) + + flag_msg = Bool() + flag_msg.data = result.is_blurred + self._pub_flag.publish(flag_msg) + + score_msg = Float32() + score_msg.data = float(result.variance) + self._pub_score.publish(score_msg) + + +def main(args=None) -> None: + rclpy.init(args=args) + node = BlurDetectNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_bringup/setup.py b/jetson/ros2_ws/src/saltybot_bringup/setup.py index a54320f..71f4da8 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/setup.py +++ b/jetson/ros2_ws/src/saltybot_bringup/setup.py @@ -41,6 +41,8 @@ setup( 'depth_hole_fill = saltybot_bringup.depth_hole_fill_node:main', # HSV color object segmenter (Issue #274) 'color_segmenter = saltybot_bringup.color_segment_node:main', + # Motion blur detector (Issue #286) + 'blur_detector = saltybot_bringup.blur_detect_node:main', ], }, ) diff --git a/jetson/ros2_ws/src/saltybot_bringup/test/test_blur_detector.py b/jetson/ros2_ws/src/saltybot_bringup/test/test_blur_detector.py new file mode 100644 index 0000000..576a312 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/test/test_blur_detector.py @@ -0,0 +1,257 @@ +""" +test_blur_detector.py — Unit tests for blur detection helpers (no ROS2 required). + +Covers: + BlurResult: + - fields accessible by name + - is_blurred True when variance < threshold + - is_blurred False when variance >= threshold + + laplacian_variance: + - sharp synthetic image has higher variance than blurred version + - solid (zero-texture) image returns near-zero variance + - greyscale input accepted without error + - ROI crop returns variance of cropped region only + - empty ROI returns 0.0 + - returns float + - variance is non-negative + + detect_blur — output contract: + - returns BlurResult + - threshold stored in result matches input + - variance matches laplacian_variance independently computed + - is_blurred consistent with variance vs threshold + + detect_blur — sharp vs blurred: + - sharp checkerboard → not blurred at default threshold + - heavily Gaussian-blurred image → blurred at default threshold + - sharp image variance > blurred image variance + - very low threshold → sharp image reported blurred + - very high threshold → blurred image still reported blurred + + detect_blur — ROI: + - sharp ROI on blurred image → ROI variant has higher variance + - blurred ROI on sharp image → ROI variant has lower variance + - roi_frac=0 equivalent to no ROI +""" + +import sys +import os + +import numpy as np +import pytest + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) + +from saltybot_bringup._blur_detector import ( + BlurResult, + laplacian_variance, + detect_blur, +) + + +# ── Image factories ─────────────────────────────────────────────────────────── + +def _checkerboard(h=64, w=64, tile=8) -> np.ndarray: + """High-frequency checkerboard — very sharp, high Laplacian variance.""" + img = np.zeros((h, w), dtype=np.uint8) + for r in range(h): + for c in range(w): + if ((r // tile) + (c // tile)) % 2 == 0: + img[r, c] = 255 + bgr = np.stack([img, img, img], axis=-1) + return bgr + + +def _blurred(bgr, ksize=21) -> np.ndarray: + """Gaussian-blur a BGR image.""" + import cv2 + k = ksize if ksize % 2 == 1 else ksize + 1 + return cv2.GaussianBlur(bgr, (k, k), 0) + + +def _solid(val=128, h=64, w=64) -> np.ndarray: + """Solid grey BGR image — zero texture.""" + return np.full((h, w, 3), val, dtype=np.uint8) + + +def _gradient(h=64, w=64) -> np.ndarray: + """Horizontal linear gradient — mild texture.""" + row = np.linspace(0, 255, w, dtype=np.uint8) + img = np.tile(row, (h, 1)) + return np.stack([img, img, img], axis=-1) + + +# ── BlurResult ──────────────────────────────────────────────────────────────── + +class TestBlurResult: + + def test_fields_accessible(self): + r = BlurResult(variance=50.0, is_blurred=True, threshold=100.0) + assert r.variance == pytest.approx(50.0) + assert r.is_blurred is True + assert r.threshold == pytest.approx(100.0) + + def test_is_blurred_true_when_variance_below_threshold(self): + r = BlurResult(variance=30.0, is_blurred=30.0 < 100.0, threshold=100.0) + assert r.is_blurred is True + + def test_is_blurred_false_when_variance_above_threshold(self): + r = BlurResult(variance=200.0, is_blurred=200.0 < 100.0, threshold=100.0) + assert r.is_blurred is False + + +# ── laplacian_variance ──────────────────────────────────────────────────────── + +class TestLaplacianVariance: + + def test_returns_float(self): + v = laplacian_variance(_checkerboard()) + assert isinstance(v, float) + + def test_variance_non_negative(self): + for img in [_checkerboard(), _solid(), _blurred(_checkerboard())]: + assert laplacian_variance(img) >= 0.0 + + def test_sharp_higher_than_blurred(self): + sharp = _checkerboard() + blurred = _blurred(sharp, ksize=21) + assert laplacian_variance(sharp) > laplacian_variance(blurred) + + def test_solid_image_near_zero(self): + v = laplacian_variance(_solid(128)) + assert v < 1.0, f'solid image variance should be near 0, got {v}' + + def test_greyscale_input_accepted(self): + grey = np.zeros((32, 32), dtype=np.uint8) + grey[::4, ::4] = 255 + v = laplacian_variance(grey) + assert isinstance(v, float) + assert v >= 0.0 + + def test_roi_returns_crop_variance(self): + """Left half: solid black. Right half: checkerboard. + ROI over right half should have high variance.""" + h, w = 64, 64 + img = np.zeros((h, w, 3), dtype=np.uint8) + cb = _checkerboard(h, w // 2, tile=4) + img[:, w // 2:] = cb + var_full = laplacian_variance(img) + var_roi = laplacian_variance(img, roi=(0, h, w // 2, w)) + assert var_roi > var_full, ( + f'ROI over checkerboard side should have higher variance: ' + f'roi={var_roi:.1f} full={var_full:.1f}' + ) + + def test_empty_roi_returns_zero(self): + img = _checkerboard() + v = laplacian_variance(img, roi=(10, 10, 5, 5)) # zero-size crop + assert v == pytest.approx(0.0) + + def test_checkerboard_has_high_variance(self): + v = laplacian_variance(_checkerboard(64, 64, tile=4)) + assert v > 500.0, f'checkerboard should have high variance, got {v:.1f}' + + +# ── detect_blur — output contract ───────────────────────────────────────────── + +class TestDetectBlurContract: + + def test_returns_blur_result(self): + r = detect_blur(_checkerboard()) + assert isinstance(r, BlurResult) + + def test_threshold_stored_in_result(self): + r = detect_blur(_checkerboard(), threshold=77.5) + assert r.threshold == pytest.approx(77.5) + + def test_variance_matches_independent_computation(self): + img = _checkerboard() + r = detect_blur(img, threshold=100.0) + expected = laplacian_variance(img) + assert r.variance == pytest.approx(expected, rel=1e-5) + + def test_is_blurred_consistent_with_variance(self): + img = _checkerboard() + r = detect_blur(img, threshold=100.0) + assert r.is_blurred == (r.variance < r.threshold) + + +# ── detect_blur — sharp vs blurred ──────────────────────────────────────────── + +class TestDetectBlurSharpVsBlurred: + + def test_checkerboard_not_blurred_at_default_threshold(self): + r = detect_blur(_checkerboard(), threshold=100.0) + assert not r.is_blurred, ( + f'sharp checkerboard should not be blurred, variance={r.variance:.1f}') + + def test_heavily_blurred_image_is_blurred(self): + blurred = _blurred(_checkerboard(), ksize=31) + r = detect_blur(blurred, threshold=100.0) + assert r.is_blurred, ( + f'heavily blurred image should be flagged, variance={r.variance:.1f}') + + def test_sharp_variance_greater_than_blurred(self): + sharp = _checkerboard() + blurred = _blurred(sharp, ksize=21) + r_sharp = detect_blur(sharp) + r_blurred = detect_blur(blurred) + assert r_sharp.variance > r_blurred.variance + + def test_very_high_threshold_flags_sharp_as_blurred(self): + """If threshold is set above any realistic variance, even sharp images are flagged.""" + r = detect_blur(_checkerboard(), threshold=1e9) + assert r.is_blurred + + def test_very_high_threshold_flags_blurred_as_blurred(self): + blurred = _blurred(_checkerboard(), ksize=31) + r = detect_blur(blurred, threshold=1e9) + assert r.is_blurred + + def test_solid_image_is_blurred_at_default_threshold(self): + r = detect_blur(_solid(128), threshold=100.0) + assert r.is_blurred, ( + f'featureless solid should be flagged, variance={r.variance:.1f}') + + def test_gradient_variance_positive(self): + """A linear gradient has some edges → variance > 0.""" + r = detect_blur(_gradient()) + assert r.variance > 0.0 + + +# ── detect_blur — ROI ──────────────────────────────────────────────────────── + +class TestDetectBlurROI: + + def test_sharp_roi_on_blurred_image_has_higher_variance(self): + """Create an image where one half is sharp and the other blurred.""" + h, w = 64, 128 + sharp_half = _checkerboard(h, w // 2, tile=4) + blurred_half = _blurred(sharp_half, ksize=21) + img = np.concatenate([sharp_half, blurred_half], axis=1) + + var_sharp_roi = laplacian_variance(img, roi=(0, h, 0, w // 2)) + var_blurred_roi = laplacian_variance(img, roi=(0, h, w // 2, w)) + assert var_sharp_roi > var_blurred_roi, ( + f'sharp ROI ({var_sharp_roi:.1f}) should exceed blurred ROI ({var_blurred_roi:.1f})' + ) + + def test_detect_blur_roi_param_passed_through(self): + """detect_blur with roi gives same variance as laplacian_variance with same roi.""" + img = _checkerboard() + roi = (0, 32, 0, 32) + r = detect_blur(img, threshold=100.0, roi=roi) + expected = laplacian_variance(img, roi=roi) + assert r.variance == pytest.approx(expected, rel=1e-5) + + def test_no_roi_uses_full_frame(self): + img = _checkerboard() + r_no_roi = detect_blur(img, threshold=100.0) + r_full_roi = detect_blur(img, threshold=100.0, + roi=(0, img.shape[0], 0, img.shape[1])) + assert r_no_roi.variance == pytest.approx(r_full_roi.variance, rel=1e-5) + + +if __name__ == '__main__': + pytest.main([__file__, '-v'])