diff --git "a/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" "b/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" index 89542ff..d3d77e5 100644 --- "a/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" +++ "b/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" @@ -173,6 +173,12 @@ ■ 画像処理パラメータ（GUI で調整可能） + GUI のコンボボックスで検出手法を切り替えられる． + 手法ごとに使用するパラメータが異なり， + GUI では選択中の手法に関連するパラメータのみ表示される． + 詳細は `TECH_04_線検出精度向上方針.txt` を参照する． + + 1. 現行手法（CLAHE + 固定閾値） ・画像解像度: 320x240 ・CLAHE 強度（clahe_clip）: 2.0 ・CLAHE 分割数（clahe_grid）: 8 @@ -182,6 +188,30 @@ ・クロージング横幅（close_width）: 25 ・クロージング高さ（close_height）: 3 + 2. 案A: Black-hat 中心型 + ・Black-hat カーネルサイズ（blackhat_ksize）: 45 + ・二値化閾値（binary_thresh）: 80 + ・等方クロージングサイズ（iso_close_size）: 15 + ・距離変換閾値（dist_thresh）: 3.0 + ・最小線幅（min_line_width）: 3 + + 3. 案B: 二重正規化型 + ・背景ブラーカーネルサイズ（bg_blur_ksize）: 101 + ・適応的閾値ブロックサイズ（adaptive_block）: 51 + ・適応的閾値定数 C（adaptive_c）: 10 + ・等方クロージングサイズ（iso_close_size）: 15 + ・距離変換閾値（dist_thresh）: 3.0 + ・最小線幅（min_line_width）: 3 + + 4. 案C: 最高ロバスト型 + ・Black-hat カーネルサイズ（blackhat_ksize）: 45 + ・適応的閾値ブロックサイズ（adaptive_block）: 51 + ・適応的閾値定数 C（adaptive_c）: 10 + ・等方クロージングサイズ（iso_close_size）: 15 + ・距離変換閾値（dist_thresh）: 3.0 + ・最小線幅（min_line_width）: 3 + ・RANSAC 閾値（ransac_thresh）: 5.0 + ■ PD 制御パラメータ（GUI で調整可能） ・Kp（位置偏差ゲイン）: 0.5 diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index f43b77f..949fa7d 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -34,6 +34,7 @@ load_entries, ) from pc.vision.line_detector import ( + DETECT_METHODS, ImageParams, LineDetectResult, detect_line, @@ -227,55 +228,227 @@ ) -> None: """画像処理パラメータ調整 UI を構築する""" group = QGroupBox("画像処理パラメータ") - form = QFormLayout() - group.setLayout(form) + layout = QVBoxLayout() + group.setLayout(layout) ip = self._pd_control.image_params + # 検出手法の選択コンボボックス + self._method_combo = QComboBox() + for key, label in DETECT_METHODS.items(): + self._method_combo.addItem(label, key) + idx = self._method_combo.findData(ip.method) + if idx >= 0: + self._method_combo.setCurrentIndex(idx) + layout.addWidget(self._method_combo) + + # パラメータフォーム + self._image_form = QFormLayout() + layout.addLayout(self._image_form) + + # 各パラメータの可視性マッピング + # (widget, 表示する手法の集合) + self._image_param_vis: list[ + tuple[QWidget, set[str]] + ] = [] + + # --- 現行手法パラメータ --- self._spin_clahe_clip = self._create_spin( ip.clahe_clip, 0.5, 10.0, 0.5, ) - form.addRow("CLAHE強度:", self._spin_clahe_clip) + self._add_image_row( + "CLAHE強度:", self._spin_clahe_clip, + {"current"}, + ) self._spin_binary_thresh = QSpinBox() self._spin_binary_thresh.setRange(10, 200) - self._spin_binary_thresh.setValue(ip.binary_thresh) - form.addRow("二値化閾値:", self._spin_binary_thresh) + self._spin_binary_thresh.setValue( + ip.binary_thresh, + ) + self._add_image_row( + "二値化閾値:", self._spin_binary_thresh, + {"current", "blackhat"}, + ) self._spin_open_size = QSpinBox() self._spin_open_size.setRange(1, 31) self._spin_open_size.setSingleStep(2) self._spin_open_size.setValue(ip.open_size) - form.addRow("ノイズ除去:", self._spin_open_size) + self._add_image_row( + "ノイズ除去:", self._spin_open_size, + {"current"}, + ) self._spin_close_width = QSpinBox() self._spin_close_width.setRange(1, 51) self._spin_close_width.setSingleStep(2) self._spin_close_width.setValue(ip.close_width) - form.addRow("途切れ補間:", self._spin_close_width) + self._add_image_row( + "途切れ補間:", self._spin_close_width, + {"current"}, + ) - # パラメータ変更時のコールバック - for spin in [ - self._spin_clahe_clip, - self._spin_binary_thresh, - self._spin_open_size, - self._spin_close_width, - ]: - spin.valueChanged.connect( + # --- 案A/C: Black-hat --- + self._spin_blackhat_ksize = QSpinBox() + self._spin_blackhat_ksize.setRange(11, 101) + self._spin_blackhat_ksize.setSingleStep(2) + self._spin_blackhat_ksize.setValue( + ip.blackhat_ksize, + ) + self._add_image_row( + "BHカーネル:", self._spin_blackhat_ksize, + {"blackhat", "robust"}, + ) + + # --- 案B: 背景除算 --- + self._spin_bg_blur_ksize = QSpinBox() + self._spin_bg_blur_ksize.setRange(31, 201) + self._spin_bg_blur_ksize.setSingleStep(2) + self._spin_bg_blur_ksize.setValue( + ip.bg_blur_ksize, + ) + self._add_image_row( + "背景ブラー:", self._spin_bg_blur_ksize, + {"dual_norm"}, + ) + + # --- 案B/C: 適応的閾値 --- + self._spin_adaptive_block = QSpinBox() + self._spin_adaptive_block.setRange(11, 101) + self._spin_adaptive_block.setSingleStep(2) + self._spin_adaptive_block.setValue( + ip.adaptive_block, + ) + self._add_image_row( + "適応ブロック:", self._spin_adaptive_block, + {"dual_norm", "robust"}, + ) + + self._spin_adaptive_c = QSpinBox() + self._spin_adaptive_c.setRange(1, 30) + self._spin_adaptive_c.setValue(ip.adaptive_c) + self._add_image_row( + "適応定数C:", self._spin_adaptive_c, + {"dual_norm", "robust"}, + ) + + # --- 案A/B/C: 後処理 --- + self._spin_iso_close = QSpinBox() + self._spin_iso_close.setRange(1, 51) + self._spin_iso_close.setSingleStep(2) + self._spin_iso_close.setValue( + ip.iso_close_size, + ) + self._add_image_row( + "穴埋め:", self._spin_iso_close, + {"blackhat", "dual_norm", "robust"}, + ) + + self._spin_dist_thresh = self._create_spin( + ip.dist_thresh, 0.0, 10.0, 0.5, + ) + self._add_image_row( + "距離閾値:", self._spin_dist_thresh, + {"blackhat", "dual_norm", "robust"}, + ) + + self._spin_min_line_width = QSpinBox() + self._spin_min_line_width.setRange(1, 20) + self._spin_min_line_width.setValue( + ip.min_line_width, + ) + self._add_image_row( + "最小線幅:", self._spin_min_line_width, + {"blackhat", "dual_norm", "robust"}, + ) + + # --- 案C: 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"}, + ) + + # コールバック接続 + self._method_combo.currentIndexChanged.connect( + self._on_method_changed, + ) + for widget, _ in self._image_param_vis: + widget.valueChanged.connect( self._on_image_param_changed, ) parent_layout.addWidget(group) + # 初期表示の更新 + self._on_method_changed() + + def _add_image_row( + self, + label: str, + widget: QWidget, + methods: set[str], + ) -> None: + """画像処理パラメータの行を追加する""" + self._image_form.addRow(label, widget) + self._image_param_vis.append( + (widget, methods), + ) + + def _on_method_changed(self) -> None: + """検出手法の変更を反映する""" + method = self._method_combo.currentData() + self._pd_control.image_params.method = method + + for widget, methods in self._image_param_vis: + visible = method in methods + widget.setVisible(visible) + label = self._image_form.labelForField( + widget, + ) + if label: + label.setVisible(visible) + def _on_image_param_changed(self) -> None: """画像処理パラメータの変更を反映する""" ip = self._pd_control.image_params + # 現行手法 ip.clahe_clip = self._spin_clahe_clip.value() ip.binary_thresh = ( self._spin_binary_thresh.value() ) ip.open_size = self._spin_open_size.value() ip.close_width = self._spin_close_width.value() + # 案A/C: Black-hat + ip.blackhat_ksize = ( + self._spin_blackhat_ksize.value() + ) + # 案B: 背景除算 + ip.bg_blur_ksize = ( + self._spin_bg_blur_ksize.value() + ) + # 案B/C: 適応的閾値 + ip.adaptive_block = ( + self._spin_adaptive_block.value() + ) + ip.adaptive_c = self._spin_adaptive_c.value() + # 案A/B/C: 後処理 + ip.iso_close_size = ( + self._spin_iso_close.value() + ) + ip.dist_thresh = ( + self._spin_dist_thresh.value() + ) + ip.min_line_width = ( + self._spin_min_line_width.value() + ) + # 案C: RANSAC + ip.ransac_thresh = ( + self._spin_ransac_thresh.value() + ) def _setup_param_store_ui( self, parent_layout: QVBoxLayout, @@ -363,6 +536,13 @@ self._spin_speed_k.setValue(p.speed_k) ip = entry.image_params + + # 検出手法 + idx = self._method_combo.findData(ip.method) + if idx >= 0: + self._method_combo.setCurrentIndex(idx) + + # 現行手法 self._spin_clahe_clip.setValue(ip.clahe_clip) self._spin_binary_thresh.setValue( ip.binary_thresh, @@ -370,6 +550,36 @@ self._spin_open_size.setValue(ip.open_size) self._spin_close_width.setValue(ip.close_width) + # 案A/C: Black-hat + self._spin_blackhat_ksize.setValue( + ip.blackhat_ksize, + ) + + # 案B: 背景除算 + self._spin_bg_blur_ksize.setValue( + ip.bg_blur_ksize, + ) + + # 案B/C: 適応的閾値 + self._spin_adaptive_block.setValue( + ip.adaptive_block, + ) + self._spin_adaptive_c.setValue(ip.adaptive_c) + + # 案A/B/C: 後処理 + self._spin_iso_close.setValue( + ip.iso_close_size, + ) + self._spin_dist_thresh.setValue(ip.dist_thresh) + self._spin_min_line_width.setValue( + ip.min_line_width, + ) + + # 案C: RANSAC + self._spin_ransac_thresh.setValue( + ip.ransac_thresh, + ) + def _on_save_param(self) -> None: """現在のパラメータを保存する""" title, ok = QInputDialog.getText( @@ -400,6 +610,9 @@ speed_k=self._spin_speed_k.value(), ), image_params=ImageParams( + method=( + self._method_combo.currentData() + ), clahe_clip=( self._spin_clahe_clip.value() ), @@ -412,6 +625,30 @@ close_width=( self._spin_close_width.value() ), + blackhat_ksize=( + self._spin_blackhat_ksize.value() + ), + bg_blur_ksize=( + self._spin_bg_blur_ksize.value() + ), + adaptive_block=( + self._spin_adaptive_block.value() + ), + adaptive_c=( + self._spin_adaptive_c.value() + ), + iso_close_size=( + self._spin_iso_close.value() + ), + dist_thresh=( + self._spin_dist_thresh.value() + ), + min_line_width=( + self._spin_min_line_width.value() + ), + ransac_thresh=( + self._spin_ransac_thresh.value() + ), ), ) add_entry(entry) diff --git a/src/pc/steering/param_store.py b/src/pc/steering/param_store.py index 892eab5..5efd5f3 100644 --- a/src/pc/steering/param_store.py +++ b/src/pc/steering/param_store.py @@ -48,9 +48,17 @@ entries: list[ParamEntry] = [] for item in data: - image_params = ImageParams( - **item["image_params"], - ) if "image_params" in item else ImageParams() + if "image_params" in item: + # 未知のフィールドを無視（後方互換性） + known = ImageParams.__dataclass_fields__ + ip_data = { + k: v + for k, v in item["image_params"].items() + if k in known + } + image_params = ImageParams(**ip_data) + else: + image_params = ImageParams() entries.append(ParamEntry( title=item["title"], diff --git a/src/pc/vision/line_detector.py b/src/pc/vision/line_detector.py index d20a263..b5c3b1b 100644 --- a/src/pc/vision/line_detector.py +++ b/src/pc/vision/line_detector.py @@ -1,8 +1,7 @@ """ line_detector カメラ画像から黒線の位置を検出するモジュール -二値化画像の白ピクセルに多項式をフィッティングして -線の位置・傾き・曲率を算出する +複数の検出手法を切り替えて使用できる """ from dataclasses import dataclass @@ -16,8 +15,17 @@ DETECT_Y_START: int = 0 DETECT_Y_END: int = config.FRAME_HEIGHT -# フィッティングに必要な最小ピクセル数 +# フィッティングに必要な最小数 MIN_FIT_PIXELS: int = 50 +MIN_FIT_ROWS: int = 10 + +# 検出手法の定義（キー: 識別子，値: 表示名） +DETECT_METHODS: dict[str, str] = { + "current": "現行（CLAHE + 固定閾値）", + "blackhat": "案A（Black-hat 中心）", + "dual_norm": "案B（二重正規化）", + "robust": "案C（最高ロバスト）", +} @dataclass @@ -25,14 +33,29 @@ """画像処理パラメータ Attributes: + method: 検出手法の識別子 clahe_clip: CLAHE のコントラスト増幅上限 clahe_grid: CLAHE の局所領域分割数 blur_size: ガウシアンブラーのカーネルサイズ（奇数） binary_thresh: 二値化の閾値 - open_size: オープニングのカーネルサイズ（孤立ノイズ除去） - close_width: クロージングの横幅（途切れ補間） + open_size: オープニングのカーネルサイズ + close_width: クロージングの横幅 close_height: クロージングの高さ + blackhat_ksize: Black-hat のカーネルサイズ + bg_blur_ksize: 背景除算のブラーカーネルサイズ + adaptive_block: 適応的閾値のブロックサイズ + adaptive_c: 適応的閾値の定数 C + iso_close_size: 等方クロージングのカーネルサイズ + dist_thresh: 距離変換の閾値 + min_line_width: 行ごと中心抽出の最小線幅 + ransac_thresh: RANSAC の外れ値判定閾値 + ransac_iter: RANSAC の反復回数 """ + + # 検出手法 + method: str = "current" + + # 現行手法パラメータ clahe_clip: float = 2.0 clahe_grid: int = 8 blur_size: int = 5 @@ -41,6 +64,25 @@ close_width: int = 25 close_height: int = 3 + # 案A/C: Black-hat + blackhat_ksize: int = 45 + + # 案B: 背景除算 + bg_blur_ksize: int = 101 + + # 案B/C: 適応的閾値 + adaptive_block: int = 51 + adaptive_c: int = 10 + + # 案A/B/C: 後処理 + iso_close_size: int = 15 + dist_thresh: float = 3.0 + min_line_width: int = 3 + + # 案C: RANSAC + ransac_thresh: float = 5.0 + ransac_iter: int = 50 + @dataclass class LineDetectResult: @@ -54,6 +96,7 @@ poly_coeffs: 多項式の係数（描画用，未検出時は None） binary_image: 二値化後の画像（デバッグ用） """ + detected: bool position_error: float heading: float @@ -68,12 +111,11 @@ ) -> LineDetectResult: """画像から黒線の位置を検出する - 二値化後の白ピクセルに2次多項式をフィッティングし， - 線の位置・傾き・曲率を算出する + params.method に応じて検出手法を切り替える Args: frame: BGR 形式のカメラ画像 - params: 画像処理パラメータ（None の場合はデフォルト） + params: 画像処理パラメータ（None でデフォルト） Returns: 線検出の結果 @@ -81,31 +123,48 @@ if params is None: params = ImageParams() - # グレースケール変換 + method = params.method + if method == "blackhat": + return _detect_blackhat(frame, params) + if method == "dual_norm": + return _detect_dual_norm(frame, params) + if method == "robust": + return _detect_robust(frame, params) + return _detect_current(frame, params) + + +# ── 検出手法の実装 ───────────────────────────── + + +def _detect_current( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """現行手法: CLAHE + 固定閾値 + 全ピクセルフィッティング""" gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # CLAHE でコントラスト強調 clahe = cv2.createCLAHE( clipLimit=params.clahe_clip, tileGridSize=( - params.clahe_grid, params.clahe_grid, + params.clahe_grid, + params.clahe_grid, ), ) enhanced = clahe.apply(gray) - # ガウシアンブラー（カーネルサイズは奇数に補正） + # ガウシアンブラー blur_k = params.blur_size | 1 blurred = cv2.GaussianBlur( enhanced, (blur_k, blur_k), 0, ) - # 固定閾値で二値化（黒線を白，背景を黒に反転） + # 固定閾値で二値化（黒線を白に反転） _, binary = cv2.threshold( blurred, params.binary_thresh, 255, cv2.THRESH_BINARY_INV, ) - # オープニング（収縮→膨張で孤立ノイズを除去） + # オープニング（孤立ノイズ除去） if params.open_size >= 3: open_k = params.open_size | 1 open_kernel = cv2.getStructuringElement( @@ -115,7 +174,7 @@ binary, cv2.MORPH_OPEN, open_kernel, ) - # 横方向クロージング（反射で途切れた線を左右からつなぐ） + # 横方向クロージング（途切れ補間） if params.close_width >= 3: close_h = max(params.close_height | 1, 1) close_kernel = cv2.getStructuringElement( @@ -126,31 +185,348 @@ binary, cv2.MORPH_CLOSE, close_kernel, ) - # 検出領域内の白ピクセル座標を取得 + # 全ピクセルフィッティング（従来方式） + return _fit_all_pixels(binary) + + +def _detect_blackhat( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案A: Black-hat 中心型 + + Black-hat 変換で背景より暗い構造を直接抽出し， + 固定閾値 + 距離変換 + 行ごと中心抽出で検出する + """ + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # Black-hat 変換（暗い構造の抽出） + bh_k = params.blackhat_ksize | 1 + bh_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (bh_k, bh_k), + ) + blackhat = cv2.morphologyEx( + gray, cv2.MORPH_BLACKHAT, bh_kernel, + ) + + # ガウシアンブラー + blur_k = params.blur_size | 1 + blurred = cv2.GaussianBlur( + blackhat, (blur_k, blur_k), 0, + ) + + # 固定閾値（Black-hat 後は線が白） + _, binary = cv2.threshold( + blurred, params.binary_thresh, 255, + cv2.THRESH_BINARY, + ) + + # 等方クロージング + 距離変換マスク + binary = _apply_iso_closing( + binary, params.iso_close_size, + ) + binary = _apply_dist_mask( + binary, params.dist_thresh, + ) + + # 行ごと中心抽出 + フィッティング + return _fit_row_centers( + binary, params.min_line_width, + ) + + +def _detect_dual_norm( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案B: 二重正規化型 + + 背景除算で照明勾配を除去し， + 適応的閾値で局所ムラにも対応する二重防壁構成 + """ + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # 背景除算正規化 + bg_k = params.bg_blur_ksize | 1 + bg = cv2.GaussianBlur( + gray, (bg_k, bg_k), 0, + ) + normalized = ( + gray.astype(np.float32) * 255.0 + / (bg.astype(np.float32) + 1.0) + ) + normalized = np.clip( + normalized, 0, 255, + ).astype(np.uint8) + + # 適応的閾値（ガウシアン，BINARY_INV） + block = max(params.adaptive_block | 1, 3) + binary = cv2.adaptiveThreshold( + normalized, 255, + cv2.ADAPTIVE_THRESH_GAUSSIAN_C, + cv2.THRESH_BINARY_INV, + block, params.adaptive_c, + ) + + # 等方クロージング + 距離変換マスク + binary = _apply_iso_closing( + binary, params.iso_close_size, + ) + binary = _apply_dist_mask( + binary, params.dist_thresh, + ) + + # 行ごと中心抽出 + フィッティング + return _fit_row_centers( + binary, params.min_line_width, + ) + + +def _detect_robust( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案C: 最高ロバスト型 + + Black-hat + 適応的閾値の二重正規化に加え， + RANSAC で外れ値を除去する最もロバストな構成 + """ + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # Black-hat 変換 + bh_k = params.blackhat_ksize | 1 + bh_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (bh_k, bh_k), + ) + blackhat = cv2.morphologyEx( + gray, cv2.MORPH_BLACKHAT, bh_kernel, + ) + + # 適応的閾値（BINARY: Black-hat 後は線が白） + block = max(params.adaptive_block | 1, 3) + binary = cv2.adaptiveThreshold( + blackhat, 255, + cv2.ADAPTIVE_THRESH_GAUSSIAN_C, + cv2.THRESH_BINARY, + block, -params.adaptive_c, + ) + + # 等方クロージング + 距離変換マスク + binary = _apply_iso_closing( + binary, params.iso_close_size, + ) + binary = _apply_dist_mask( + binary, params.dist_thresh, + ) + + # 行ごと中央値抽出 + RANSAC フィッティング + return _fit_row_centers( + binary, params.min_line_width, + use_median=True, + ransac_thresh=params.ransac_thresh, + ransac_iter=params.ransac_iter, + ) + + +# ── 共通処理 ─────────────────────────────────── + + +def _apply_iso_closing( + binary: np.ndarray, size: int, +) -> np.ndarray: + """等方クロージングで穴を埋める + + Args: + binary: 二値画像 + size: カーネルサイズ + + Returns: + クロージング後の二値画像 + """ + if size < 3: + return binary + k = size | 1 + kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (k, k), + ) + return cv2.morphologyEx( + binary, cv2.MORPH_CLOSE, kernel, + ) + + +def _apply_dist_mask( + binary: np.ndarray, thresh: float, +) -> np.ndarray: + """距離変換で中心部のみを残す + + Args: + binary: 二値画像 + thresh: 距離の閾値（ピクセル） + + Returns: + 中心部のみの二値画像 + """ + if thresh <= 0: + return binary + dist = cv2.distanceTransform( + binary, cv2.DIST_L2, 5, + ) + _, mask = cv2.threshold( + dist, thresh, 255, cv2.THRESH_BINARY, + ) + return mask.astype(np.uint8) + + +def _fit_all_pixels( + binary: np.ndarray, +) -> LineDetectResult: + """全白ピクセルに多項式をフィッティングする + + 従来方式．全ピクセルを等しく扱うため， + 陰で幅が広がった行がフィッティングを支配する弱点がある + + Args: + binary: 二値画像 + + Returns: + 線検出の結果 + """ region = binary[DETECT_Y_START:DETECT_Y_END, :] ys_local, xs = np.where(region > 0) if len(xs) < MIN_FIT_PIXELS: - return LineDetectResult( - detected=False, - position_error=0.0, - heading=0.0, - curvature=0.0, - poly_coeffs=None, - binary_image=binary, - ) + return _no_detection(binary) - # 全体画像座標に変換 ys = ys_local + DETECT_Y_START - - # 2次多項式フィッティング: x = f(y) = ay² + by + c coeffs = np.polyfit(ys, xs, 2) - poly = np.poly1d(coeffs) + return _build_result(coeffs, binary) - # 画像中心 + +def _fit_row_centers( + binary: np.ndarray, + min_width: int, + use_median: bool = False, + ransac_thresh: float = 0.0, + ransac_iter: int = 0, +) -> LineDetectResult: + """行ごとの中心点に多項式をフィッティングする + + 各行の白ピクセルの中心（平均または中央値）を1点抽出し， + 中心点列に対してフィッティングする． + 幅の変動に強く，各行が等しく寄与する + + Args: + binary: 二値画像 + min_width: 線として認識する最小ピクセル数 + use_median: True の場合は中央値を使用 + ransac_thresh: RANSAC 閾値（0 以下で無効） + ransac_iter: RANSAC 反復回数 + + Returns: + 線検出の結果 + """ + region = binary[DETECT_Y_START:DETECT_Y_END, :] + centers_y: list[float] = [] + centers_x: list[float] = [] + + for y_local in range(region.shape[0]): + xs = np.where(region[y_local] > 0)[0] + if len(xs) < min_width: + continue + y = float(y_local + DETECT_Y_START) + centers_y.append(y) + if use_median: + centers_x.append(float(np.median(xs))) + else: + centers_x.append(float(np.mean(xs))) + + if len(centers_y) < MIN_FIT_ROWS: + return _no_detection(binary) + + 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) + + return _build_result(coeffs, binary) + + +def _ransac_polyfit( + ys: np.ndarray, xs: np.ndarray, + degree: int, n_iter: int, thresh: float, +) -> np.ndarray | None: + """RANSAC で外れ値を除去して多項式フィッティング + + Args: + ys: y 座標配列 + xs: x 座標配列 + degree: 多項式の次数 + n_iter: 反復回数 + thresh: 外れ値判定閾値（ピクセル） + + Returns: + 多項式係数（フィッティング失敗時は None） + """ + n = len(ys) + sample_size = degree + 1 + if n < sample_size: + return None + + best_coeffs: np.ndarray | None = None + best_inliers = 0 + rng = np.random.default_rng() + + for _ in range(n_iter): + idx = rng.choice(n, sample_size, replace=False) + coeffs = np.polyfit(ys[idx], xs[idx], degree) + poly = np.poly1d(coeffs) + residuals = np.abs(xs - poly(ys)) + n_inliers = int(np.sum(residuals < thresh)) + if n_inliers > best_inliers: + best_inliers = n_inliers + best_coeffs = coeffs + + # インライアで再フィッティング + if best_coeffs is not None: + poly = np.poly1d(best_coeffs) + inlier_mask = np.abs(xs - poly(ys)) < thresh + if np.sum(inlier_mask) >= sample_size: + best_coeffs = np.polyfit( + ys[inlier_mask], + xs[inlier_mask], + degree, + ) + + return best_coeffs + + +def _no_detection( + binary: np.ndarray, +) -> LineDetectResult: + """未検出の結果を返す""" + return LineDetectResult( + detected=False, + position_error=0.0, + heading=0.0, + curvature=0.0, + poly_coeffs=None, + binary_image=binary, + ) + + +def _build_result( + coeffs: np.ndarray, + binary: np.ndarray, +) -> LineDetectResult: + """多項式係数から LineDetectResult を構築する""" + poly = np.poly1d(coeffs) center_x = config.FRAME_WIDTH / 2.0 - # 画像下端（近方）での位置偏差 + # 画像下端での位置偏差 x_bottom = poly(DETECT_Y_END) position_error = (center_x - x_bottom) / center_x @@ -158,7 +534,7 @@ poly_deriv = poly.deriv() heading = float(poly_deriv(DETECT_Y_END)) - # 曲率: d²x/dy²（2次多項式では定数 = 2a） + # 曲率: d²x/dy² poly_deriv2 = poly_deriv.deriv() curvature = float(poly_deriv2(DETECT_Y_END))