diff --git "a/docs/03_TECH/TECH_04_\347\267\232\346\244\234\345\207\272\347\262\276\345\272\246\345\220\221\344\270\212\346\226\271\351\207\235.txt" "b/docs/03_TECH/TECH_04_\347\267\232\346\244\234\345\207\272\347\262\276\345\272\246\345\220\221\344\270\212\346\226\271\351\207\235.txt" index afea42c..54cd2dc 100644 --- "a/docs/03_TECH/TECH_04_\347\267\232\346\244\234\345\207\272\347\262\276\345\272\246\345\220\221\344\270\212\346\226\271\351\207\235.txt" +++ "b/docs/03_TECH/TECH_04_\347\267\232\346\244\234\345\207\272\347\262\276\345\272\246\345\220\221\344\270\212\346\226\271\351\207\235.txt" @@ -785,3 +785,89 @@ ・評価環境: 通常照明,強照明,照明ムラありの3条件を推奨 ・計測項目: 周回数,コースアウト回数,position_error の分散 + + +11. 案D: 谷検出+追跡型 (Valley Detection + Tracking) +------------------------------------------------------------------------ + + 11-1. 概要 + + 案A〜Cはいずれも二値化(固定閾値または適応的閾値)を経由するが, + 光の反射や影によって二値化の閾値設定が困難になる場面がある. + 案Dは二値化を完全に排除し,各行の輝度信号から直接「谷」(暗い + 領域)を検出することで,照明変動に対する根本的な耐性を得る. + + さらに,時系列追跡(トラッキング)により検出の安定性を確保し, + 一時的な検出失敗にも対応できる構成とする. + + 11-2. アルゴリズム + + ■ 谷検出(行ごと,フレーム単位) + + (1) 画像全体を水平方向にガウシアンブラーする(カーネルサイズ: + valley_gauss_ksize) + (2) 各行の輝度信号から極小値(谷)を検出する + (3) 各谷について以下を計算する: + - 谷の中心 x 座標: 左右の肩の中点 + - 谷の深度: 左右の肩の平均輝度 - 谷底の輝度 + - 谷の幅: 左の肩から右の肩までのピクセル数 + (4) 以下のフィルタで不正な谷を除去する: + - 最小深度フィルタ(valley_min_depth) + - 透視補正付き幅フィルタ(width_near, width_far, width_tolerance) + - 予測偏差フィルタ(valley_max_deviation,後述の追跡と連動) + (5) 残った谷のうち最もスコアの高いものを採用する + スコア = 深度 + 予測位置への近さ + + ■ 時系列追跡(フレーム間) + + (1) 前フレームの多項式係数から各行の x 座標を予測する + (2) 予測から大きく外れた谷候補を棄却する(予測偏差フィルタ) + (3) 検出成功時: 多項式係数を指数移動平均(EMA)で平滑化する + smoothed = alpha × current + (1 - alpha) × prev + (4) 検出失敗時: 前フレームの予測で補間する(コースティング) + valley_coast_frames フレームまで継続し,超えると未検出を返す + + 11-3. 案A〜Cとの違い + + ・二値化が不要: 輝度の相対的な谷を検出するため,閾値設定の問題 + が根本的に解消される + ・時系列情報を活用: 案A〜Cは各フレームを独立に処理するが, + 案Dは前フレームの結果を予測・平滑化に活用する + ・検出失敗への耐性: コースティングにより数フレームの検出失敗を + 許容できる(案A〜Cは即座に未検出となる) + + 11-4. パラメータ一覧 + + ・valley_gauss_ksize (デフォルト: 15) + 行ごとのガウシアン平滑化カーネルサイズ. + 大きくするとノイズに強くなるが,細い線を検出しにくくなる + ・valley_min_depth (デフォルト: 15) + 谷として認識する最小深度(輝度差). + 小さくすると感度が上がるが誤検出も増える + ・valley_max_deviation (デフォルト: 40) + 追跡予測からの最大許容偏差(px). + 小さくすると追跡が安定するが急カーブへの追従が遅れる + ・valley_coast_frames (デフォルト: 3) + 検出失敗時に予測を継続するフレーム数. + 大きくすると一時的な見失いに強くなるが, + 誤った予測で走行し続けるリスクも増える + ・valley_ema_alpha (デフォルト: 0.7) + 多項式係数の EMA 係数.1.0 で平滑化なし, + 小さくすると安定するがラグが増える + ・ransac_thresh / ransac_iter(案Cと共通) + 有効にすると谷検出結果に RANSAC を適用する + ・width_near / width_far / width_tolerance(共通) + 透視補正付き幅フィルタ.谷の幅の検証に使用する + + 11-5. 実装ファイル + + ・src/pc/vision/line_detector.py + - ValleyTracker クラス: 時系列追跡の状態管理 + - _find_row_valley(): 1行の谷検出 + - _detect_valley(): 案Dのメイン処理 + - _build_valley_binary(): デバッグ用二値画像生成 + - reset_valley_tracker(): 追跡状態のリセット + ・src/pc/gui/main_window.py + - 案D用パラメータの GUI コントロール + ・src/pc/steering/pd_control.py + - reset() 時に追跡状態もリセット diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index 10101f4..6715293 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -50,6 +50,7 @@ ImageParams, LineDetectResult, detect_line, + reset_valley_tracker, ) from pc.vision.overlay import OverlayFlags, draw_overlay @@ -428,13 +429,13 @@ {"blackhat", "dual_norm", "robust"}, ) - # --- 案C: RANSAC --- + # --- 案C/D: RANSAC --- self._spin_ransac_thresh = self._create_spin( ip.ransac_thresh, 1.0, 30.0, 1.0, ) self._add_image_row( "RANSAC閾値:", self._spin_ransac_thresh, - {"robust"}, + {"robust", "valley"}, ) # --- 幅フィルタ(透視補正) --- @@ -444,7 +445,7 @@ self._spin_width_near.setSpecialValueText("無効") self._add_image_row( "線幅(近)px:", self._spin_width_near, - {"blackhat", "dual_norm", "robust"}, + {"blackhat", "dual_norm", "robust", "valley"}, ) self._spin_width_far = QSpinBox() @@ -453,7 +454,7 @@ self._spin_width_far.setSpecialValueText("無効") self._add_image_row( "線幅(遠)px:", self._spin_width_far, - {"blackhat", "dual_norm", "robust"}, + {"blackhat", "dual_norm", "robust", "valley"}, ) self._spin_width_tolerance = self._create_spin( @@ -461,7 +462,57 @@ ) self._add_image_row( "幅フィルタ倍率:", self._spin_width_tolerance, - {"blackhat", "dual_norm", "robust"}, + {"blackhat", "dual_norm", "robust", "valley"}, + ) + + # --- 案D: 谷検出+追跡 --- + self._spin_valley_gauss = QSpinBox() + self._spin_valley_gauss.setRange(3, 51) + self._spin_valley_gauss.setSingleStep(2) + self._spin_valley_gauss.setValue( + ip.valley_gauss_ksize, + ) + self._add_image_row( + "谷ガウス:", self._spin_valley_gauss, + {"valley"}, + ) + + self._spin_valley_min_depth = QSpinBox() + self._spin_valley_min_depth.setRange(1, 100) + self._spin_valley_min_depth.setValue( + ip.valley_min_depth, + ) + self._add_image_row( + "最小谷深度:", self._spin_valley_min_depth, + {"valley"}, + ) + + self._spin_valley_max_dev = QSpinBox() + self._spin_valley_max_dev.setRange(5, 200) + self._spin_valley_max_dev.setValue( + ip.valley_max_deviation, + ) + self._add_image_row( + "最大偏差:", self._spin_valley_max_dev, + {"valley"}, + ) + + self._spin_valley_coast = QSpinBox() + self._spin_valley_coast.setRange(0, 10) + self._spin_valley_coast.setValue( + ip.valley_coast_frames, + ) + self._add_image_row( + "予測継続:", self._spin_valley_coast, + {"valley"}, + ) + + self._spin_valley_ema = self._create_spin( + ip.valley_ema_alpha, 0.0, 1.0, 0.05, + ) + self._add_image_row( + "EMA係数:", self._spin_valley_ema, + {"valley"}, ) # --- プリセット管理 --- @@ -531,6 +582,9 @@ """検出手法の変更を反映する""" method = self._method_combo.currentData() + # 谷検出の追跡状態をリセット + reset_valley_tracker() + # 旧手法のパラメータを保存 if self._auto_save_enabled: ip = self._pd_control.image_params @@ -607,6 +661,22 @@ self._spin_width_tolerance.setValue( ip.width_tolerance, ) + # 案D: 谷検出+追跡 + self._spin_valley_gauss.setValue( + ip.valley_gauss_ksize, + ) + self._spin_valley_min_depth.setValue( + ip.valley_min_depth, + ) + self._spin_valley_max_dev.setValue( + ip.valley_max_deviation, + ) + self._spin_valley_coast.setValue( + ip.valley_coast_frames, + ) + self._spin_valley_ema.setValue( + ip.valley_ema_alpha, + ) finally: self._auto_save_enabled = True @@ -653,6 +723,22 @@ ip.width_tolerance = ( self._spin_width_tolerance.value() ) + # 案D: 谷検出+追跡 + ip.valley_gauss_ksize = ( + self._spin_valley_gauss.value() + ) + ip.valley_min_depth = ( + self._spin_valley_min_depth.value() + ) + ip.valley_max_deviation = ( + self._spin_valley_max_dev.value() + ) + ip.valley_coast_frames = ( + self._spin_valley_coast.value() + ) + ip.valley_ema_alpha = ( + self._spin_valley_ema.value() + ) if self._auto_save_enabled: save_detect_params(ip.method, ip) diff --git a/src/pc/steering/pd_control.py b/src/pc/steering/pd_control.py index 1f3a7fc..753c8e6 100644 --- a/src/pc/steering/pd_control.py +++ b/src/pc/steering/pd_control.py @@ -10,7 +10,11 @@ import numpy as np from pc.steering.base import SteeringBase, SteeringOutput -from pc.vision.line_detector import ImageParams, detect_line +from pc.vision.line_detector import ( + ImageParams, + detect_line, + reset_valley_tracker, +) @dataclass @@ -121,6 +125,7 @@ self._prev_time = 0.0 self._prev_steer = 0.0 self._last_result = None + reset_valley_tracker() @property def last_detect_result(self): diff --git a/src/pc/vision/line_detector.py b/src/pc/vision/line_detector.py index dde4275..03fa121 100644 --- a/src/pc/vision/line_detector.py +++ b/src/pc/vision/line_detector.py @@ -25,6 +25,7 @@ "blackhat": "案A(Black-hat 中心)", "dual_norm": "案B(二重正規化)", "robust": "案C(最高ロバスト)", + "valley": "案D(谷検出+追跡)", } @@ -53,6 +54,11 @@ width_near: 画像下端での期待線幅(px,0 で無効) width_far: 画像上端での期待線幅(px,0 で無効) width_tolerance: 幅フィルタの上限倍率 + valley_gauss_ksize: 谷検出の行ごとガウシアンカーネルサイズ + valley_min_depth: 谷として認識する最小深度 + valley_max_deviation: 追跡予測からの最大許容偏差(px) + valley_coast_frames: 検出失敗時の予測継続フレーム数 + valley_ema_alpha: 多項式係数の指数移動平均係数 """ # 検出手法 @@ -91,6 +97,13 @@ width_far: int = 0 width_tolerance: float = 1.8 + # 案D: 谷検出+追跡 + valley_gauss_ksize: int = 15 + valley_min_depth: int = 15 + valley_max_deviation: int = 40 + valley_coast_frames: int = 3 + valley_ema_alpha: float = 0.7 + @dataclass class LineDetectResult: @@ -138,6 +151,8 @@ return _detect_dual_norm(frame, params) if method == "robust": return _detect_robust(frame, params) + if method == "valley": + return _detect_valley(frame, params) return _detect_current(frame, params) @@ -608,3 +623,316 @@ poly_coeffs=coeffs, binary_image=binary, ) + + +# ── 案D: 谷検出+追跡 ───────────────────────────── + + +class ValleyTracker: + """谷検出の時系列追跡を管理するクラス + + 前フレームの多項式係数を保持し,予測・平滑化・ + 検出失敗時のコースティングを提供する + """ + + def __init__(self) -> None: + self._prev_coeffs: np.ndarray | None = None + self._smoothed_coeffs: np.ndarray | None = None + self._frames_lost: int = 0 + + def predict_x(self, y: float) -> float | None: + """前フレームの多項式から x 座標を予測する + + Args: + y: 画像の y 座標 + + Returns: + 予測 x 座標(履歴なしの場合は None) + """ + if self._smoothed_coeffs is None: + return None + return float(np.poly1d(self._smoothed_coeffs)(y)) + + def update( + self, + coeffs: np.ndarray, + alpha: float, + ) -> np.ndarray: + """検出成功時に状態を更新する + + EMA で多項式係数を平滑化し,更新後の係数を返す + + Args: + coeffs: 今フレームのフィッティング係数 + alpha: EMA 係数(1.0 で平滑化なし) + + Returns: + 平滑化後の多項式係数 + """ + self._frames_lost = 0 + if self._smoothed_coeffs is None: + self._smoothed_coeffs = coeffs.copy() + else: + self._smoothed_coeffs = ( + alpha * coeffs + + (1.0 - alpha) * self._smoothed_coeffs + ) + self._prev_coeffs = self._smoothed_coeffs.copy() + return self._smoothed_coeffs + + def coast( + self, max_frames: int, + ) -> LineDetectResult | None: + """検出失敗時に予測結果を返す + + Args: + max_frames: 予測を継続する最大フレーム数 + + Returns: + 予測による結果(継続不可の場合は None) + """ + if self._smoothed_coeffs is None: + return None + self._frames_lost += 1 + if self._frames_lost > max_frames: + return None + # 予測でデバッグ用二値画像は空にする + h = config.FRAME_HEIGHT + w = config.FRAME_WIDTH + blank = np.zeros((h, w), dtype=np.uint8) + return _build_result(self._smoothed_coeffs, blank) + + def reset(self) -> None: + """追跡状態をリセットする""" + self._prev_coeffs = None + self._smoothed_coeffs = None + self._frames_lost = 0 + + +_valley_tracker = ValleyTracker() + + +def reset_valley_tracker() -> None: + """谷検出の追跡状態をリセットする""" + _valley_tracker.reset() + + +def _find_row_valley( + row: np.ndarray, + min_depth: int, + expected_width: float, + width_tolerance: float, + predicted_x: float | None, + max_deviation: int, +) -> tuple[float, float] | None: + """1行の輝度信号から最適な谷を検出する + + Args: + row: スムージング済みの1行輝度信号 + min_depth: 最小谷深度 + expected_width: 期待線幅(px,0 で幅フィルタ無効) + width_tolerance: 幅フィルタの上限倍率 + predicted_x: 追跡による予測 x 座標(None で無効) + max_deviation: 予測からの最大許容偏差 + + Returns: + (谷の中心x, 谷の深度) または None + """ + n = len(row) + if n < 5: + return None + + signal = row.astype(np.float32) + + # 極小値を検出(前後より小さい点) + left = signal[:-2] + center = signal[1:-1] + right = signal[2:] + minima_mask = (center <= left) & (center <= right) + minima_indices = np.where(minima_mask)[0] + 1 + + if len(minima_indices) == 0: + return None + + best: tuple[float, float] | None = None + best_score = -1.0 + + for idx in minima_indices: + val = signal[idx] + + # 左の肩を探す + left_shoulder = idx + for i in range(idx - 1, -1, -1): + if signal[i] < signal[i + 1]: + break + left_shoulder = i + # 右の肩を探す + right_shoulder = idx + for i in range(idx + 1, n): + if signal[i] < signal[i - 1]: + break + right_shoulder = i + + # 谷の深度(肩の平均 - 谷底) + shoulder_avg = ( + signal[left_shoulder] + signal[right_shoulder] + ) / 2.0 + depth = shoulder_avg - val + if depth < min_depth: + continue + + # 谷の幅 + width = right_shoulder - left_shoulder + center_x = (left_shoulder + right_shoulder) / 2.0 + + # 幅フィルタ + if expected_width > 0: + max_w = expected_width * width_tolerance + min_w = expected_width / width_tolerance + if width > max_w or width < min_w: + continue + + # 予測との偏差チェック + if predicted_x is not None: + if abs(center_x - predicted_x) > max_deviation: + continue + + # スコア: 深度優先,予測がある場合は近さも考慮 + score = float(depth) + if predicted_x is not None: + dist = abs(center_x - predicted_x) + score += max(0.0, max_deviation - dist) + + if score > best_score: + best_score = score + best = (center_x, float(depth)) + + return best + + +def _build_valley_binary( + shape: tuple[int, int], + centers_y: list[int], + centers_x: list[float], +) -> np.ndarray: + """谷検出結果からデバッグ用二値画像を生成する + + Args: + shape: 出力画像の (高さ, 幅) + centers_y: 検出行の y 座標リスト + centers_x: 検出行の中心 x 座標リスト + + Returns: + デバッグ用二値画像 + """ + binary = np.zeros(shape, dtype=np.uint8) + half_w = 3 + w = shape[1] + for y, cx in zip(centers_y, centers_x): + x0 = max(0, int(cx) - half_w) + x1 = min(w, int(cx) + half_w + 1) + binary[y, x0:x1] = 255 + return binary + + +def _detect_valley( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案D: 谷検出+追跡型 + + 各行の輝度信号から谷(暗い領域)を直接検出し, + 時系列追跡で安定性を確保する.二値化を使用しない + """ + h, w = frame.shape[:2] + + # 行ごとにガウシアン平滑化するため画像全体をブラー + gauss_k = params.valley_gauss_ksize | 1 + blurred = cv2.GaussianBlur( + frame, (gauss_k, 1), 0, + ) + + # 透視補正の期待幅を計算するための準備 + use_width = ( + params.width_near > 0 and params.width_far > 0 + ) + detect_h = DETECT_Y_END - DETECT_Y_START + denom = max(detect_h - 1, 1) + + centers_y: list[int] = [] + centers_x: list[float] = [] + + for y in range(DETECT_Y_START, DETECT_Y_END): + row = blurred[y] + + # 期待幅の計算 + if use_width: + t = (DETECT_Y_END - 1 - y) / denom + expected_w = float(params.width_far) + ( + float(params.width_near) + - float(params.width_far) + ) * t + else: + expected_w = 0.0 + + # 予測 x 座標 + predicted_x = _valley_tracker.predict_x( + float(y), + ) + + result = _find_row_valley( + row, + params.valley_min_depth, + expected_w, + params.width_tolerance, + predicted_x, + params.valley_max_deviation, + ) + if result is not None: + centers_y.append(y) + centers_x.append(result[0]) + + # デバッグ用二値画像 + debug_binary = _build_valley_binary( + (h, w), centers_y, centers_x, + ) + + if len(centers_y) < MIN_FIT_ROWS: + # 検出失敗 → コースティングを試みる + coasted = _valley_tracker.coast( + params.valley_coast_frames, + ) + if coasted is not None: + coasted.binary_image = debug_binary + return coasted + return _no_detection(debug_binary) + + cy = np.array(centers_y, dtype=np.float64) + cx = np.array(centers_x, dtype=np.float64) + + # RANSAC が有効な場合は使用 + if ( + params.ransac_thresh > 0 + and params.ransac_iter > 0 + ): + coeffs = _ransac_polyfit( + cy, cx, 2, + params.ransac_iter, + params.ransac_thresh, + ) + if coeffs is None: + coasted = _valley_tracker.coast( + params.valley_coast_frames, + ) + if coasted is not None: + coasted.binary_image = debug_binary + return coasted + return _no_detection(debug_binary) + else: + coeffs = np.polyfit(cy, cx, 2) + + # EMA で平滑化 + smoothed = _valley_tracker.update( + coeffs, params.valley_ema_alpha, + ) + + return _build_result(smoothed, debug_binary) diff --git a/src/pi/camera/capture.py b/src/pi/camera/capture.py index 712e7c4..3e63916 100644 --- a/src/pi/camera/capture.py +++ b/src/pi/camera/capture.py @@ -21,7 +21,7 @@ camera_config = self._camera.create_preview_configuration( main={ "size": (config.FRAME_WIDTH, config.FRAME_HEIGHT), - "format": "Y8", + "format": "YUV420", }, ) self._camera.configure(camera_config) @@ -33,7 +33,8 @@ Returns: グレースケールの画像(NumPy 配列) """ - return self._camera.capture_array() + yuv = self._camera.capture_array() + return yuv[:config.FRAME_HEIGHT, :config.FRAME_WIDTH] def stop(self) -> None: """カメラを停止する"""