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 54cd2dc..f715361 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" @@ -591,6 +591,31 @@ ・備考: S 字カーブ等,2次多項式では表現できない 複雑な形状に対応できる.ただし過学習のリスクがある + 7-7. ロバストフィッティング前処理(全手法共通) + + 行ごと中心抽出の後,多項式フィッティングの前に適用する + 外れ値除去パイプライン.全検出手法(案A〜D)で共通に使用する + + (1) 移動メディアンフィルタ(median_ksize) + 中心点列の x 座標に1次元メディアンフィルタを適用し, + スパイク状の外れ値を平滑化する + (2) 近傍外れ値除去(neighbor_thresh) + 各点の x 座標を近傍(前後3行)の中央値と比較し, + 閾値を超えて逸脱する点を除去する. + グローバルなモデルに依存しないローカルな判定 + (3) 重み付き最小二乗 + 案Dでは谷の深度(コントラスト)を重みとして使用し, + 深度が浅い(=信頼度の低い)点の影響を低減する + + ・パラメータ: + - median_ksize: メディアンフィルタのカーネルサイズ + (デフォルト: 7,0 で無効) + - neighbor_thresh: 近傍除去の閾値 px + (デフォルト: 10.0,0 で無効) + ・実装: _clean_and_fit() 関数(line_detector.py) + ・備考: RANSAC と併用可能.RANSAC が有効な場合は + メディアン → 近傍除去 → RANSAC の順に適用される + 8. Stage 0: 撮影条件の最適化 (Camera Settings) ------------------------------------------------------------------------ diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index 6715293..6c9b0a9 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -429,6 +429,31 @@ {"blackhat", "dual_norm", "robust"}, ) + # --- ロバストフィッティング(全手法共通) --- + all_methods = { + "blackhat", "dual_norm", "robust", "valley", + } + + self._spin_median_ksize = QSpinBox() + self._spin_median_ksize.setRange(0, 31) + self._spin_median_ksize.setSingleStep(2) + self._spin_median_ksize.setValue(ip.median_ksize) + self._spin_median_ksize.setSpecialValueText( + "無効", + ) + self._add_image_row( + "メディアン:", self._spin_median_ksize, + all_methods, + ) + + self._spin_neighbor_thresh = self._create_spin( + ip.neighbor_thresh, 0.0, 50.0, 1.0, + ) + self._add_image_row( + "近傍除去:", self._spin_neighbor_thresh, + all_methods, + ) + # --- 案C/D: RANSAC --- self._spin_ransac_thresh = self._create_spin( ip.ransac_thresh, 1.0, 30.0, 1.0, @@ -653,6 +678,12 @@ self._spin_min_line_width.setValue( ip.min_line_width, ) + self._spin_median_ksize.setValue( + ip.median_ksize, + ) + self._spin_neighbor_thresh.setValue( + ip.neighbor_thresh, + ) self._spin_ransac_thresh.setValue( ip.ransac_thresh, ) @@ -713,6 +744,13 @@ ip.min_line_width = ( self._spin_min_line_width.value() ) + # ロバストフィッティング + ip.median_ksize = ( + self._spin_median_ksize.value() + ) + ip.neighbor_thresh = ( + self._spin_neighbor_thresh.value() + ) # 案C: RANSAC ip.ransac_thresh = ( self._spin_ransac_thresh.value() diff --git a/src/pc/vision/line_detector.py b/src/pc/vision/line_detector.py index 03fa121..2433580 100644 --- a/src/pc/vision/line_detector.py +++ b/src/pc/vision/line_detector.py @@ -54,6 +54,8 @@ width_near: 画像下端での期待線幅(px,0 で無効) width_far: 画像上端での期待線幅(px,0 で無効) width_tolerance: 幅フィルタの上限倍率 + median_ksize: 中心点列の移動メディアンフィルタサイズ(0 で無効) + neighbor_thresh: 近傍外れ値除去の閾値(px,0 で無効) valley_gauss_ksize: 谷検出の行ごとガウシアンカーネルサイズ valley_min_depth: 谷として認識する最小深度 valley_max_deviation: 追跡予測からの最大許容偏差(px) @@ -92,6 +94,10 @@ ransac_thresh: float = 5.0 ransac_iter: int = 50 + # ロバストフィッティング(全手法共通) + median_ksize: int = 7 + neighbor_thresh: float = 10.0 + # 透視補正付き幅フィルタ(0 で無効) width_near: int = 0 width_far: int = 0 @@ -257,6 +263,8 @@ # 行ごと中心抽出 + フィッティング return _fit_row_centers( binary, params.min_line_width, + median_ksize=params.median_ksize, + neighbor_thresh=params.neighbor_thresh, ) @@ -308,6 +316,8 @@ # 行ごと中心抽出 + フィッティング return _fit_row_centers( binary, params.min_line_width, + median_ksize=params.median_ksize, + neighbor_thresh=params.neighbor_thresh, ) @@ -358,6 +368,8 @@ use_median=True, ransac_thresh=params.ransac_thresh, ransac_iter=params.ransac_iter, + median_ksize=params.median_ksize, + neighbor_thresh=params.neighbor_thresh, ) @@ -482,11 +494,13 @@ use_median: bool = False, ransac_thresh: float = 0.0, ransac_iter: int = 0, + median_ksize: int = 0, + neighbor_thresh: float = 0.0, ) -> LineDetectResult: """行ごとの中心点に多項式をフィッティングする 各行の白ピクセルの中心(平均または中央値)を1点抽出し, - 中心点列に対してフィッティングする. + ロバスト前処理の後にフィッティングする. 幅の変動に強く,各行が等しく寄与する Args: @@ -495,6 +509,8 @@ use_median: True の場合は中央値を使用 ransac_thresh: RANSAC 閾値(0 以下で無効) ransac_iter: RANSAC 反復回数 + median_ksize: 移動メディアンのカーネルサイズ + neighbor_thresh: 近傍外れ値除去の閾値 px Returns: 線検出の結果 @@ -520,14 +536,15 @@ cy = np.array(centers_y) cx = np.array(centers_x) - if ransac_thresh > 0 and ransac_iter > 0: - coeffs = _ransac_polyfit( - cy, cx, 2, ransac_iter, ransac_thresh, - ) - if coeffs is None: - return _no_detection(binary) - else: - coeffs = np.polyfit(cy, cx, 2) + coeffs = _clean_and_fit( + cy, cx, + median_ksize=median_ksize, + neighbor_thresh=neighbor_thresh, + ransac_thresh=ransac_thresh, + ransac_iter=ransac_iter, + ) + if coeffs is None: + return _no_detection(binary) return _build_result(coeffs, binary) @@ -581,6 +598,80 @@ return best_coeffs +def _clean_and_fit( + cy: np.ndarray, + cx: np.ndarray, + median_ksize: int, + neighbor_thresh: float, + weights: np.ndarray | None = None, + ransac_thresh: float = 0.0, + ransac_iter: int = 0, +) -> np.ndarray | None: + """外れ値除去+重み付きフィッティングを行う + + 全検出手法で共通に使えるロバストなフィッティング. + (1) 移動メディアンフィルタでスパイクを平滑化 + (2) 近傍中央値からの偏差で外れ値を除去 + (3) 重み付き最小二乗(または RANSAC)でフィッティング + + Args: + cy: 中心点の y 座標配列 + cx: 中心点の x 座標配列 + median_ksize: 移動メディアンのカーネルサイズ(0 で無効) + neighbor_thresh: 近傍外れ値除去の閾値 px(0 で無効) + weights: 各点の信頼度(None で均等) + ransac_thresh: RANSAC 閾値(0 以下で無効) + ransac_iter: RANSAC 反復回数 + + Returns: + 多項式係数(フィッティング失敗時は None) + """ + if len(cy) < MIN_FIT_ROWS: + return None + + cx_clean = cx.copy() + mask = np.ones(len(cy), dtype=bool) + + # (1) 移動メディアンフィルタ + if median_ksize >= 3: + k = median_ksize | 1 + half = k // 2 + for i in range(len(cx_clean)): + lo = max(0, i - half) + hi = min(len(cx_clean), i + half + 1) + cx_clean[i] = float(np.median(cx[lo:hi])) + + # (2) 近傍外れ値除去 + if neighbor_thresh > 0: + half_n = 3 + for i in range(len(cx_clean)): + lo = max(0, i - half_n) + hi = min(len(cx_clean), i + half_n + 1) + local_med = float(np.median(cx_clean[lo:hi])) + if abs(cx_clean[i] - local_med) > neighbor_thresh: + mask[i] = False + + cy = cy[mask] + cx_clean = cx_clean[mask] + if weights is not None: + weights = weights[mask] + + if len(cy) < MIN_FIT_ROWS: + return None + + # (3) フィッティング + if ransac_thresh > 0 and ransac_iter > 0: + return _ransac_polyfit( + cy, cx_clean, 2, ransac_iter, ransac_thresh, + ) + + # 重み付き最小二乗 + if weights is not None: + return np.polyfit(cy, cx_clean, 2, w=weights) + + return np.polyfit(cy, cx_clean, 2) + + def _no_detection( binary: np.ndarray, ) -> LineDetectResult: @@ -860,6 +951,7 @@ centers_y: list[int] = [] centers_x: list[float] = [] + depths: list[float] = [] for y in range(DETECT_Y_START, DETECT_Y_END): row = blurred[y] @@ -890,6 +982,7 @@ if result is not None: centers_y.append(y) centers_x.append(result[0]) + depths.append(result[1]) # デバッグ用二値画像 debug_binary = _build_valley_binary( @@ -908,27 +1001,25 @@ cy = np.array(centers_y, dtype=np.float64) cx = np.array(centers_x, dtype=np.float64) + w_arr = np.array(depths, 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, + # ロバストフィッティング(深度を重みに使用) + coeffs = _clean_and_fit( + cy, cx, + median_ksize=params.median_ksize, + neighbor_thresh=params.neighbor_thresh, + weights=w_arr, + ransac_thresh=params.ransac_thresh, + ransac_iter=params.ransac_iter, + ) + if coeffs is None: + coasted = _valley_tracker.coast( + params.valley_coast_frames, ) - 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) + if coasted is not None: + coasted.binary_image = debug_binary + return coasted + return _no_detection(debug_binary) # EMA で平滑化 smoothed = _valley_tracker.update(