diff --git a/deploy.sh b/deploy.sh index ba1ed4d..4cb5aa2 100644 --- a/deploy.sh +++ b/deploy.sh @@ -14,7 +14,7 @@ # ── Pi 側の既存フォルダを削除 ───────────────────────────── echo "Pi 側のフォルダを初期化中..." -ssh "${PI_HOST}" "rm -rf ${PI_DIR}/common ${PI_DIR}/pi" +ssh "${PI_HOST}" "rm -rf ${PI_DIR}/common ${PI_DIR}/pi ${PI_DIR}/params" # ── ファイル転送 ────────────────────────────────────────── echo "common/ を転送中..." @@ -23,6 +23,10 @@ echo "pi/ を転送中..." scp -r "${SRC_DIR}/pi" "${PI_HOST}:${PI_DIR}/" +# ── モデルファイルの転送 ──────────────────────────────────── +echo "params/ を転送中..." +scp -r "${SCRIPT_DIR}/params" "${PI_HOST}:${PI_DIR}/" + # ── 設定ファイルの転送 ──────────────────────────────────── echo ".env を転送中..." scp "${SCRIPT_DIR}/.env" "${PI_HOST}:${PI_DIR}/.env" diff --git "a/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" "b/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" index a6f4a5a..525b5fd 100644 --- "a/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" +++ "b/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" @@ -99,11 +99,19 @@ ├── motor/ モーター関連 │ └── driver.py TB6612FNG 制御 ├── vision/ 画像処理（Pi 側で実行） - │ ├── line_detector.py 線検出（現行手法のみ） - │ ├── fitting.py 直線・曲線近似 + │ ├── line_detector.py 線検出 API（手法ディスパッチ） + │ ├── fitting.py 直線・曲線近似（Theil-Sen・RANSAC） │ ├── morphology.py 形態学的処理ユーティリティ - │ └── intersection.py 十字路分類モデルの推論 + │ ├── intersection.py 十字路分類モデルの推論 + │ └── detectors/ 検出手法の実装 + │ ├── current.py 現行（CLAHE + 固定閾値） + │ ├── blackhat.py 案A（Black-hat 中心） + │ ├── dual_norm.py 案B（二重正規化） + │ ├── robust.py 案C（最高ロバスト） + │ └── valley.py 案D（谷検出+追跡） └── steering/ 操舵量計算（Pi 側で実行） ├── base.py 共通インターフェース + ├── pd_control.py PD 制御の実装 + ├── pursuit_control.py 2点パシュート制御の実装 ├── ts_pd_control.py Theil-Sen PD 制御の実装 └── recovery.py コースアウト復帰 diff --git a/requirements_pi.txt b/requirements_pi.txt index 8b753d4..2b73096 100644 --- a/requirements_pi.txt +++ b/requirements_pi.txt @@ -2,3 +2,7 @@ picamera2 RPi.GPIO python-dotenv==1.2.2 +opencv-python-headless +numpy +scikit-learn +joblib diff --git a/src/common/json_utils.py b/src/common/json_utils.py index 0ce705a..969c5f0 100644 --- a/src/common/json_utils.py +++ b/src/common/json_utils.py @@ -7,9 +7,21 @@ from pathlib import Path # プロジェクトルートの params/ ディレクトリ -PARAMS_DIR: Path = ( - Path(__file__).resolve().parent.parent.parent / "params" -) +# PC では src/common/ の3階層上，Pi では common/ の2階層上になるため +# 上方向に探索して params/ を見つける +def _find_params_dir() -> Path: + """params/ ディレクトリを上方向に探索して返す""" + d = Path(__file__).resolve().parent + while d != d.parent: + candidate = d / "params" + if candidate.is_dir(): + return candidate + d = d.parent + # 見つからない場合はデフォルト + return Path(__file__).resolve().parent.parent / "params" + + +PARAMS_DIR: Path = _find_params_dir() def read_json(path: Path) -> dict: diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index a17de83..e777710 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -330,9 +330,10 @@ def _on_pursuit_params_changed( self, p: PursuitParams, ) -> None: - """Pursuit パラメータの変更を保存する""" + """Pursuit パラメータの変更を保存・マークする""" self._pursuit_params = p save_pursuit(p) + self._params_dirty = True def _on_ts_pd_params_changed( self, p: TsPdParams, @@ -678,11 +679,20 @@ self._intersection_panel.throttle ) + # 制御手法 + cmd["steering_method"] = self._steering_method + # パラメータ更新（変更があった場合のみ） if self._params_dirty: cmd["image_params"] = dataclasses.asdict( self._image_params, ) + cmd["pd_params"] = dataclasses.asdict( + self._pd_params, + ) + cmd["pursuit_params"] = dataclasses.asdict( + self._pursuit_params, + ) cmd["steering_params"] = dataclasses.asdict( self._ts_pd_params, ) diff --git a/src/pi/comm/zmq_client.py b/src/pi/comm/zmq_client.py index bb0c701..4f272de 100644 --- a/src/pi/comm/zmq_client.py +++ b/src/pi/comm/zmq_client.py @@ -137,9 +137,12 @@ mode: "auto" | "manual" | "stop" throttle: float（手動モード時のみ） steer: float（手動モード時のみ） - image_params: dict（パラメータ更新，省略可） - steering_params: dict（パラメータ更新，省略可） - recovery_params: dict（パラメータ更新，省略可） + steering_method: "pd" | "pursuit" | "ts_pd" + image_params: dict（二値化パラメータ更新，省略可） + pd_params: dict（PD 制御パラメータ，省略可） + pursuit_params: dict（Pursuit パラメータ，省略可） + steering_params: dict（TS-PD パラメータ，省略可） + recovery_params: dict（復帰パラメータ，省略可） intersection_enabled: bool（省略可） intersection_throttle: float（省略可） diff --git a/src/pi/main.py b/src/pi/main.py index 8b38581..8739db3 100644 --- a/src/pi/main.py +++ b/src/pi/main.py @@ -13,7 +13,12 @@ from pi.camera.capture import CameraCapture from pi.comm.zmq_client import PiZmqClient from pi.motor.driver import MotorDriver -from pi.steering.base import SteeringOutput +from pi.steering.base import SteeringBase, SteeringOutput +from pi.steering.pd_control import PdControl, PdParams +from pi.steering.pursuit_control import ( + PursuitControl, + PursuitParams, +) from pi.steering.recovery import ( RecoveryController, RecoveryParams, @@ -32,8 +37,13 @@ zmq_client = PiZmqClient() motor = MotorDriver() - # 操舵制御 - steering = TsPdControl() + # 操舵制御（3手法） + pd_control = PdControl() + pursuit_control = PursuitControl() + ts_pd_control = TsPdControl() + steering: SteeringBase = ts_pd_control + steering_method = "ts_pd" + recovery = RecoveryController() # 十字路分類器（遅延読み込み） @@ -62,8 +72,28 @@ # ── コマンド受信 ──────────────────────── cmd = zmq_client.receive_command() if cmd is not None: + # 制御手法の切り替え + new_method = cmd.get("steering_method") + if ( + new_method is not None + and new_method != steering_method + ): + steering_method = new_method + if steering_method == "pd": + steering = pd_control + elif steering_method == "pursuit": + steering = pursuit_control + else: + steering = ts_pd_control + steering_method = "ts_pd" + print( + f"Pi: 制御手法変更 → " + f"{steering_method}" + ) + _apply_command( - cmd, steering, recovery, + cmd, pd_control, pursuit_control, + ts_pd_control, recovery, intersection_clf, ) # モード更新 @@ -106,7 +136,7 @@ binary_image = None if mode == "auto": - # 線検出 + PD 制御 + # 線検出 + 制御 output = steering.compute(frame) det = steering.last_detect_result @@ -201,7 +231,9 @@ def _apply_command( cmd: dict, - steering: TsPdControl, + pd_control: PdControl, + pursuit_control: PursuitControl, + ts_pd_control: TsPdControl, recovery: RecoveryController, intersection_clf: IntersectionClassifier, ) -> None: @@ -209,22 +241,43 @@ Args: cmd: 受信したコマンド辞書 - steering: 操舵制御クラス + pd_control: PD 制御クラス + pursuit_control: Pursuit 制御クラス + ts_pd_control: Theil-Sen PD 制御クラス recovery: 復帰制御クラス intersection_clf: 十字路分類器 """ - # 画像処理パラメータの更新 + # 画像処理パラメータの更新（全制御クラスに反映） if "image_params" in cmd: ip = cmd["image_params"] - current = steering.image_params - for key, value in ip.items(): + for ctrl in [ + pd_control, pursuit_control, ts_pd_control, + ]: + current = ctrl.image_params + for key, value in ip.items(): + if hasattr(current, key): + setattr(current, key, value) + + # PD パラメータの更新 + if "pd_params" in cmd: + sp = cmd["pd_params"] + current = pd_control.params + for key, value in sp.items(): if hasattr(current, key): setattr(current, key, value) - # 操舵パラメータの更新 + # Pursuit パラメータの更新 + if "pursuit_params" in cmd: + pp = cmd["pursuit_params"] + current = pursuit_control.params + for key, value in pp.items(): + if hasattr(current, key): + setattr(current, key, value) + + # Theil-Sen PD パラメータの更新 if "steering_params" in cmd: sp = cmd["steering_params"] - current = steering.params + current = ts_pd_control.params for key, value in sp.items(): if hasattr(current, key): setattr(current, key, value) diff --git a/src/pi/steering/pd_control.py b/src/pi/steering/pd_control.py new file mode 100644 index 0000000..4600dd9 --- /dev/null +++ b/src/pi/steering/pd_control.py @@ -0,0 +1,138 @@ +""" +pd_control +PD 制御による操舵量計算モジュール +多項式フィッティングの位置・傾き・曲率から操舵量と速度を算出する +""" + +import time +from dataclasses import dataclass + +import numpy as np + +from pi.steering.base import SteeringBase, SteeringOutput +from pi.vision.line_detector import ( + ImageParams, + LineDetectResult, + detect_line, + reset_valley_tracker, +) + + +@dataclass +class PdParams: + """PD 制御のパラメータ + + Attributes: + kp: 位置偏差ゲイン + kh: 傾き（ヘディング）ゲイン + kd: 微分ゲイン + max_steer_rate: 1フレームあたりの最大操舵変化量 + max_throttle: 直線での最大速度 + speed_k: 曲率ベースの減速係数 + """ + kp: float = 0.5 + kh: float = 0.3 + kd: float = 0.1 + max_steer_rate: float = 0.1 + max_throttle: float = 0.4 + speed_k: float = 0.3 + + +class PdControl(SteeringBase): + """PD 制御による操舵量計算クラス""" + + def __init__( + self, + params: PdParams | None = None, + image_params: ImageParams | None = None, + ) -> None: + self.params: PdParams = params or PdParams() + self.image_params: ImageParams = ( + image_params or ImageParams() + ) + self._prev_error: float = 0.0 + self._prev_time: float = 0.0 + self._prev_steer: float = 0.0 + self._last_result: LineDetectResult | None = None + + def compute( + self, frame: np.ndarray, + ) -> SteeringOutput: + """カメラ画像から PD 制御で操舵量を計算する + + Args: + frame: グレースケールのカメラ画像 + + Returns: + 計算された操舵量 + """ + p = self.params + + # 線検出 + result = detect_line(frame, self.image_params) + self._last_result = result + + # 線が検出できなかった場合は停止 + if not result.detected: + return SteeringOutput( + throttle=0.0, steer=0.0, + ) + + # 位置偏差 + 傾きによる操舵量 + error = ( + p.kp * result.position_error + + p.kh * result.heading + ) + + # 時間差分の計算 + now = time.time() + dt = ( + now - self._prev_time + if self._prev_time > 0 + else 0.033 + ) + dt = max(dt, 0.001) + + # 微分項 + derivative = (error - self._prev_error) / dt + steer = error + p.kd * derivative + + # 操舵量のクランプ + steer = max(-1.0, min(1.0, steer)) + + # レートリミッター（急な操舵変化を制限） + delta = steer - self._prev_steer + max_delta = p.max_steer_rate + delta = max(-max_delta, min(max_delta, delta)) + steer = self._prev_steer + delta + + # 速度制御（曲率連動） + throttle = ( + p.max_throttle + - p.speed_k * abs(result.curvature) + ) + throttle = max(0.0, throttle) + + # 状態の更新 + self._prev_error = error + self._prev_time = now + self._prev_steer = steer + + return SteeringOutput( + throttle=throttle, steer=steer, + ) + + def reset(self) -> None: + """内部状態をリセットする""" + self._prev_error = 0.0 + self._prev_time = 0.0 + self._prev_steer = 0.0 + self._last_result = None + reset_valley_tracker() + + @property + def last_detect_result( + self, + ) -> LineDetectResult | None: + """直近の線検出結果を取得する""" + return self._last_result diff --git a/src/pi/steering/pursuit_control.py b/src/pi/steering/pursuit_control.py new file mode 100644 index 0000000..ee11ab4 --- /dev/null +++ b/src/pi/steering/pursuit_control.py @@ -0,0 +1,147 @@ +""" +pursuit_control +2点パシュートによる操舵量計算モジュール +行中心点に Theil-Sen 直線近似を適用し，外れ値に強い操舵量を算出する +""" + +from dataclasses import dataclass + +import numpy as np + +from common import config +from pi.steering.base import SteeringBase, SteeringOutput +from pi.vision.fitting import theil_sen_fit +from pi.vision.line_detector import ( + ImageParams, + LineDetectResult, + detect_line, + reset_valley_tracker, +) + + +@dataclass +class PursuitParams: + """2点パシュート制御のパラメータ + + Attributes: + near_ratio: 近い目標点の位置（0.0=上端，1.0=下端） + far_ratio: 遠い目標点の位置（0.0=上端，1.0=下端） + k_near: 近い目標点の操舵ゲイン + k_far: 遠い目標点の操舵ゲイン + max_steer_rate: 1フレームあたりの最大操舵変化量 + max_throttle: 直線での最大速度 + speed_k: カーブ減速係数（2点の差に対する係数） + """ + near_ratio: float = 0.8 + far_ratio: float = 0.3 + k_near: float = 0.5 + k_far: float = 0.3 + max_steer_rate: float = 0.1 + max_throttle: float = 0.4 + speed_k: float = 2.0 + + +class PursuitControl(SteeringBase): + """2点パシュートによる操舵量計算クラス + + 行中心点から Theil-Sen 直線近似を行い， + 直線上の近い点と遠い点の偏差から操舵量を計算する + """ + + def __init__( + self, + params: PursuitParams | None = None, + image_params: ImageParams | None = None, + ) -> None: + self.params: PursuitParams = ( + params or PursuitParams() + ) + self.image_params: ImageParams = ( + image_params or ImageParams() + ) + self._prev_steer: float = 0.0 + self._last_result: LineDetectResult | None = None + + def compute( + self, frame: np.ndarray, + ) -> SteeringOutput: + """カメラ画像から2点パシュートで操舵量を計算する + + Args: + frame: グレースケールのカメラ画像 + + Returns: + 計算された操舵量 + """ + p = self.params + + # 線検出 + result = detect_line(frame, self.image_params) + self._last_result = result + + if not result.detected or result.row_centers is None: + return SteeringOutput( + throttle=0.0, steer=0.0, + ) + + centers = result.row_centers + + # 有効な点（NaN でない行）を抽出 + valid = ~np.isnan(centers) + ys = np.where(valid)[0].astype(float) + xs = centers[valid] + + if len(ys) < 2: + return SteeringOutput( + throttle=0.0, steer=0.0, + ) + + # Theil-Sen 直線近似 + slope, intercept = theil_sen_fit(ys, xs) + + center_x = config.FRAME_WIDTH / 2.0 + h = len(centers) + + # 直線上の 2 点の x 座標を取得 + near_y = h * p.near_ratio + far_y = h * p.far_ratio + near_x = slope * near_y + intercept + far_x = slope * far_y + intercept + + # 各点の偏差（正: 線が左にある → 右に曲がる） + near_err = (center_x - near_x) / center_x + far_err = (center_x - far_x) / center_x + + # 操舵量 + steer = p.k_near * near_err + p.k_far * far_err + steer = max(-1.0, min(1.0, steer)) + + # レートリミッター + delta = steer - self._prev_steer + max_delta = p.max_steer_rate + delta = max(-max_delta, min(max_delta, delta)) + steer = self._prev_steer + delta + + # 速度制御（2点の x 差でカーブ度合いを判定） + curve = abs(near_x - far_x) / center_x + throttle = p.max_throttle - p.speed_k * curve + throttle = max(0.0, throttle) + + self._prev_steer = steer + + return SteeringOutput( + throttle=throttle, steer=steer, + ) + + def reset(self) -> None: + """内部状態をリセットする""" + self._prev_steer = 0.0 + self._last_result = None + reset_valley_tracker() + + @property + def last_detect_result( + self, + ) -> LineDetectResult | None: + """直近の線検出結果を取得する""" + return self._last_result diff --git a/src/pi/vision/detectors/blackhat.py b/src/pi/vision/detectors/blackhat.py new file mode 100644 index 0000000..908670d --- /dev/null +++ b/src/pi/vision/detectors/blackhat.py @@ -0,0 +1,69 @@ +""" +blackhat +案A: Black-hat 中心型の線検出 +Black-hat 変換で背景より暗い構造を直接抽出し， +固定閾値 + 距離変換 + 行ごと中心抽出で検出する +""" + +import cv2 +import numpy as np + +from pi.vision.line_detector import ( + ImageParams, + LineDetectResult, + fit_row_centers, +) +from pi.vision.morphology import ( + apply_dist_mask, + apply_iso_closing, + apply_width_filter, +) + + +def detect_blackhat( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案A: Black-hat 中心型""" + # Black-hat 変換（暗い構造の抽出） + bh_k = params.blackhat_ksize | 1 + bh_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (bh_k, bh_k), + ) + blackhat = cv2.morphologyEx( + frame, 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, + ) + if params.width_near > 0 and params.width_far > 0: + binary = apply_width_filter( + binary, + params.width_near, + params.width_far, + params.width_tolerance, + ) + + # 行ごと中心抽出 + フィッティング + return fit_row_centers( + binary, params.min_line_width, + median_ksize=params.median_ksize, + neighbor_thresh=params.neighbor_thresh, + residual_thresh=params.residual_thresh, + ) diff --git a/src/pi/vision/detectors/current.py b/src/pi/vision/detectors/current.py new file mode 100644 index 0000000..59e1c76 --- /dev/null +++ b/src/pi/vision/detectors/current.py @@ -0,0 +1,76 @@ +""" +current +現行手法: CLAHE + 固定閾値 + 全ピクセルフィッティング +""" + +import cv2 +import numpy as np + +from pi.vision.line_detector import ( + DETECT_Y_END, + DETECT_Y_START, + MIN_FIT_PIXELS, + ImageParams, + LineDetectResult, + build_result, + no_detection, +) + + +def detect_current( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """現行手法: CLAHE + 固定閾値 + 全ピクセルフィッティング""" + # CLAHE でコントラスト強調 + clahe = cv2.createCLAHE( + clipLimit=params.clahe_clip, + tileGridSize=( + params.clahe_grid, + params.clahe_grid, + ), + ) + enhanced = clahe.apply(frame) + + # ガウシアンブラー + 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( + cv2.MORPH_ELLIPSE, (open_k, open_k), + ) + binary = cv2.morphologyEx( + binary, cv2.MORPH_OPEN, open_kernel, + ) + + # 横方向クロージング（途切れ補間） + if params.close_width >= 3: + close_h = max(params.close_height | 1, 1) + close_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, + (params.close_width, close_h), + ) + binary = cv2.morphologyEx( + binary, cv2.MORPH_CLOSE, close_kernel, + ) + + # 全ピクセルフィッティング + region = binary[DETECT_Y_START:DETECT_Y_END, :] + ys_local, xs = np.where(region > 0) + + if len(xs) < MIN_FIT_PIXELS: + return no_detection(binary) + + ys = ys_local + DETECT_Y_START + coeffs = np.polyfit(ys, xs, 2) + return build_result(coeffs, binary) diff --git a/src/pi/vision/detectors/dual_norm.py b/src/pi/vision/detectors/dual_norm.py new file mode 100644 index 0000000..170b97e --- /dev/null +++ b/src/pi/vision/detectors/dual_norm.py @@ -0,0 +1,89 @@ +""" +dual_norm +案B: 二重正規化型の線検出 +背景除算で照明勾配を除去し， +適応的閾値で局所ムラにも対応する二重防壁構成 +""" + +import cv2 +import numpy as np + +from pi.vision.line_detector import ( + ImageParams, + LineDetectResult, + fit_row_centers, +) +from pi.vision.morphology import ( + apply_dist_mask, + apply_iso_closing, + apply_staged_closing, + apply_width_filter, +) + + +def detect_dual_norm( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案B: 二重正規化型""" + # 背景除算正規化 + bg_k = params.bg_blur_ksize | 1 + bg = cv2.GaussianBlur( + frame, (bg_k, bg_k), 0, + ) + normalized = ( + frame.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, + ) + + # 固定閾値との AND（有効時のみ） + if params.global_thresh > 0: + _, global_mask = cv2.threshold( + normalized, params.global_thresh, + 255, cv2.THRESH_BINARY_INV, + ) + binary = cv2.bitwise_and(binary, global_mask) + + # 段階クロージング or 等方クロージング + if params.stage_min_area > 0: + binary = apply_staged_closing( + binary, + params.stage_close_small, + params.stage_min_area, + params.stage_close_large, + ) + else: + binary = apply_iso_closing( + binary, params.iso_close_size, + ) + + # 距離変換マスク + 幅フィルタ + binary = apply_dist_mask( + binary, params.dist_thresh, + ) + if params.width_near > 0 and params.width_far > 0: + binary = apply_width_filter( + binary, + params.width_near, + params.width_far, + params.width_tolerance, + ) + + # 行ごと中心抽出 + フィッティング + return fit_row_centers( + binary, params.min_line_width, + median_ksize=params.median_ksize, + neighbor_thresh=params.neighbor_thresh, + residual_thresh=params.residual_thresh, + ) diff --git a/src/pi/vision/detectors/robust.py b/src/pi/vision/detectors/robust.py new file mode 100644 index 0000000..fa6e79f --- /dev/null +++ b/src/pi/vision/detectors/robust.py @@ -0,0 +1,69 @@ +""" +robust +案C: 最高ロバスト型の線検出 +Black-hat + 適応的閾値の二重正規化に加え， +RANSAC で外れ値を除去する最もロバストな構成 +""" + +import cv2 +import numpy as np + +from pi.vision.line_detector import ( + ImageParams, + LineDetectResult, + fit_row_centers, +) +from pi.vision.morphology import ( + apply_dist_mask, + apply_iso_closing, + apply_width_filter, +) + + +def detect_robust( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案C: 最高ロバスト型""" + # Black-hat 変換 + bh_k = params.blackhat_ksize | 1 + bh_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (bh_k, bh_k), + ) + blackhat = cv2.morphologyEx( + frame, 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, + ) + if params.width_near > 0 and params.width_far > 0: + binary = apply_width_filter( + binary, + params.width_near, + params.width_far, + params.width_tolerance, + ) + + # 行ごと中央値抽出 + RANSAC フィッティング + return fit_row_centers( + binary, params.min_line_width, + use_median=True, + ransac_thresh=params.ransac_thresh, + ransac_iter=params.ransac_iter, + median_ksize=params.median_ksize, + neighbor_thresh=params.neighbor_thresh, + residual_thresh=params.residual_thresh, + ) diff --git a/src/pi/vision/detectors/valley.py b/src/pi/vision/detectors/valley.py new file mode 100644 index 0000000..5a485db --- /dev/null +++ b/src/pi/vision/detectors/valley.py @@ -0,0 +1,324 @@ +""" +valley +案D: 谷検出+追跡型の線検出 +各行の輝度信号から谷（暗い領域）を直接検出し， +時系列追跡で安定性を確保する．二値化を使用しない +""" + +import cv2 +import numpy as np + +from common import config +from pi.vision.fitting import clean_and_fit +from pi.vision.line_detector import ( + DETECT_Y_END, + DETECT_Y_START, + MIN_FIT_ROWS, + ImageParams, + LineDetectResult, + build_result, + no_detection, +) + + +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: + """検出成功時に状態を更新する + + 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] = [] + depths: 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]) + depths.append(result[1]) + + # デバッグ用二値画像 + 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) + w_arr = np.array(depths, dtype=np.float64) + + # ロバストフィッティング（深度を重みに使用） + coeffs = clean_and_fit( + cy, cx, + median_ksize=params.median_ksize, + neighbor_thresh=params.neighbor_thresh, + residual_thresh=params.residual_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 coasted is not None: + coasted.binary_image = debug_binary + return coasted + return no_detection(debug_binary) + + # EMA で平滑化 + smoothed = _valley_tracker.update( + coeffs, params.valley_ema_alpha, + ) + + return build_result(smoothed, debug_binary) diff --git a/src/pi/vision/fitting.py b/src/pi/vision/fitting.py index d8122c0..c55139d 100644 --- a/src/pi/vision/fitting.py +++ b/src/pi/vision/fitting.py @@ -48,6 +48,55 @@ return slope, intercept +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 clean_and_fit( cy: np.ndarray, cx: np.ndarray, @@ -60,6 +109,12 @@ ) -> np.ndarray | None: """外れ値除去＋重み付きフィッティングを行う + 全検出手法で共通に使えるロバストなフィッティング + (1) 移動メディアンフィルタでスパイクを平滑化 + (2) 近傍中央値からの偏差で外れ値を除去（複数パス） + (3) 重み付き最小二乗（または RANSAC）でフィッティング + (4) 残差ベースの反復除去で外れ値を最終除去 + Args: cy: 中心点の y 座標配列 cx: 中心点の x 座標配列 @@ -103,7 +158,10 @@ new_mask[i] = False continue local_med = float(np.median(neighbors)) - if abs(cx_clean[i] - local_med) > neighbor_thresh: + if ( + abs(cx_clean[i] - local_med) + > neighbor_thresh + ): new_mask[i] = False if np.array_equal(mask, mask & new_mask): break @@ -118,11 +176,18 @@ return None # (3) フィッティング - if weights is not None: + if ransac_thresh > 0 and ransac_iter > 0: + coeffs = ransac_polyfit( + cy, cx_clean, 2, ransac_iter, ransac_thresh, + ) + elif weights is not None: coeffs = np.polyfit(cy, cx_clean, 2, w=weights) else: coeffs = np.polyfit(cy, cx_clean, 2) + if coeffs is None: + return None + # (4) 残差ベースの反復除去 if residual_thresh > 0: for _ in range(RESIDUAL_REMOVAL_ITERATIONS): diff --git a/src/pi/vision/intersection.py b/src/pi/vision/intersection.py index 46788a4..20a94e3 100644 --- a/src/pi/vision/intersection.py +++ b/src/pi/vision/intersection.py @@ -33,14 +33,23 @@ def load(self) -> None: """モデルとスケーラを読み込む（遅延呼び出し用）""" if not _MODEL_PATH.exists(): + print( + f" モデルが見つかりません: {_MODEL_PATH}" + ) return if not _SCALER_PATH.exists(): + print( + f" スケーラが見つかりません: {_SCALER_PATH}" + ) return - import joblib + try: + import joblib - self._model = joblib.load(_MODEL_PATH) - self._scaler = joblib.load(_SCALER_PATH) - self._available = True + self._model = joblib.load(_MODEL_PATH) + self._scaler = joblib.load(_SCALER_PATH) + self._available = True + except Exception as e: + print(f" モデル読み込みエラー: {e}") @property def available(self) -> bool: diff --git a/src/pi/vision/line_detector.py b/src/pi/vision/line_detector.py index 6dca0be..5de0cde 100644 --- a/src/pi/vision/line_detector.py +++ b/src/pi/vision/line_detector.py @@ -1,7 +1,11 @@ """ line_detector カメラ画像から黒線の位置を検出するモジュール -Pi 側では現行手法（current）のみ使用する +複数の検出手法を切り替えて使用できる + +公開 API: + ImageParams, LineDetectResult, detect_line, + reset_valley_tracker, DETECT_METHODS """ from dataclasses import dataclass @@ -20,12 +24,22 @@ 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（最高ロバスト）", + "valley": "案D（谷検出+追跡）", +} + @dataclass class ImageParams: """二値化パラメータ Attributes: + method: 検出手法の識別子 clahe_clip: CLAHE のコントラスト増幅上限 clahe_grid: CLAHE の局所領域分割数 blur_size: ガウシアンブラーのカーネルサイズ（奇数） @@ -33,11 +47,36 @@ open_size: オープニングのカーネルサイズ close_width: クロージングの横幅 close_height: クロージングの高さ + blackhat_ksize: Black-hat のカーネルサイズ + bg_blur_ksize: 背景除算のブラーカーネルサイズ + global_thresh: 固定閾値（0 で無効，適応的閾値との AND） + adaptive_block: 適応的閾値のブロックサイズ + adaptive_c: 適応的閾値の定数 C + iso_close_size: 等方クロージングのカーネルサイズ + dist_thresh: 距離変換の閾値 + min_line_width: 行ごと中心抽出の最小線幅 + stage_close_small: 段階クロージング第1段のサイズ + stage_min_area: 孤立除去の最小面積（0 で無効） + stage_close_large: 段階クロージング第2段のサイズ（0 で無効） + ransac_thresh: RANSAC の外れ値判定閾値 + ransac_iter: RANSAC の反復回数 + width_near: 画像下端での期待線幅（px，0 で無効） + width_far: 画像上端での期待線幅（px，0 で無効） + width_tolerance: 幅フィルタの上限倍率 median_ksize: 中心点列の移動メディアンフィルタサイズ（0 で無効） neighbor_thresh: 近傍外れ値除去の閾値（px，0 で無効） residual_thresh: 残差反復除去の閾値（px，0 で無効） + valley_gauss_ksize: 谷検出の行ごとガウシアンカーネルサイズ + valley_min_depth: 谷として認識する最小深度 + valley_max_deviation: 追跡予測からの最大許容偏差（px） + valley_coast_frames: 検出失敗時の予測継続フレーム数 + valley_ema_alpha: 多項式係数の指数移動平均係数 """ + # 検出手法 + method: str = "current" + + # 現行手法パラメータ clahe_clip: float = 2.0 clahe_grid: int = 8 blur_size: int = 5 @@ -45,10 +84,49 @@ open_size: int = 5 close_width: int = 25 close_height: int = 3 + + # 案A/C: Black-hat + blackhat_ksize: int = 45 + + # 案B: 背景除算 + bg_blur_ksize: int = 101 + global_thresh: int = 0 + + # 案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 + + # 案B: 段階クロージング + stage_close_small: int = 5 + stage_min_area: int = 0 + stage_close_large: int = 0 + + # 案C: RANSAC + ransac_thresh: float = 5.0 + ransac_iter: int = 50 + + # ロバストフィッティング（全手法共通） median_ksize: int = 7 neighbor_thresh: float = 10.0 residual_thresh: float = 8.0 + # 透視補正付き幅フィルタ（0 で無効） + width_near: int = 0 + 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: @@ -74,6 +152,66 @@ binary_image: np.ndarray | None +# ── 公開 API ────────────────────────────────────── + + +def detect_line( + frame: np.ndarray, + params: ImageParams | None = None, +) -> LineDetectResult: + """画像から黒線の位置を検出する + + params.method に応じて検出手法を切り替える + + Args: + frame: グレースケールのカメラ画像 + params: 二値化パラメータ（None でデフォルト） + + Returns: + 線検出の結果 + """ + if params is None: + params = ImageParams() + + method = params.method + if method == "blackhat": + from pi.vision.detectors.blackhat import ( + detect_blackhat, + ) + return detect_blackhat(frame, params) + if method == "dual_norm": + from pi.vision.detectors.dual_norm import ( + detect_dual_norm, + ) + return detect_dual_norm(frame, params) + if method == "robust": + from pi.vision.detectors.robust import ( + detect_robust, + ) + return detect_robust(frame, params) + if method == "valley": + from pi.vision.detectors.valley import ( + detect_valley, + ) + return detect_valley(frame, params) + + from pi.vision.detectors.current import ( + detect_current, + ) + return detect_current(frame, params) + + +def reset_valley_tracker() -> None: + """谷検出の追跡状態をリセットする""" + from pi.vision.detectors.valley import ( + reset_valley_tracker as _reset, + ) + _reset() + + +# ── 共通結果構築（各検出器から使用） ────────────── + + def no_detection( binary: np.ndarray, ) -> LineDetectResult: @@ -109,10 +247,14 @@ if num_labels <= 1: return None + # 背景（ラベル 0）を除いた最大領域を取得 areas = stats[1:, cv2.CC_STAT_AREA] largest_label = int(np.argmax(areas)) + 1 + + # 最大領域のマスク mask = (labels == largest_label).astype(np.uint8) + # 各行の左右端から中心を計算 centers = np.full(h, np.nan) for y in range(h): row = mask[y] @@ -135,15 +277,19 @@ 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 + # 傾き: dx/dy（画像下端での値） poly_deriv = poly.deriv() heading = float(poly_deriv(DETECT_Y_END)) + # 曲率: d²x/dy² poly_deriv2 = poly_deriv.deriv() curvature = float(poly_deriv2(DETECT_Y_END)) + # row_centers が未提供なら binary から抽出 if row_centers is None: row_centers = _extract_row_centers(binary) @@ -158,72 +304,61 @@ ) -def detect_line( - frame: np.ndarray, - params: ImageParams | None = None, +def fit_row_centers( + binary: np.ndarray, + min_width: int, + use_median: bool = False, + ransac_thresh: float = 0.0, + ransac_iter: int = 0, + median_ksize: int = 0, + neighbor_thresh: float = 0.0, + residual_thresh: float = 0.0, ) -> LineDetectResult: - """画像から黒線の位置を検出する（現行手法） + """行ごとの中心点に多項式をフィッティングする Args: - frame: グレースケールのカメラ画像 - params: 二値化パラメータ（None でデフォルト） + binary: 二値画像 + min_width: 線として認識する最小ピクセル数 + use_median: True の場合は中央値を使用 + ransac_thresh: RANSAC 閾値（0 以下で無効） + ransac_iter: RANSAC 反復回数 + median_ksize: 移動メディアンのカーネルサイズ + neighbor_thresh: 近傍外れ値除去の閾値 px + residual_thresh: 残差反復除去の閾値 px Returns: 線検出の結果 """ - if params is None: - params = ImageParams() - - # CLAHE でコントラスト強調 - clahe = cv2.createCLAHE( - clipLimit=params.clahe_clip, - tileGridSize=( - params.clahe_grid, - params.clahe_grid, - ), - ) - enhanced = clahe.apply(frame) - - # ガウシアンブラー - 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( - cv2.MORPH_ELLIPSE, (open_k, open_k), - ) - binary = cv2.morphologyEx( - binary, cv2.MORPH_OPEN, open_kernel, - ) - - # 横方向クロージング（途切れ補間） - if params.close_width >= 3: - close_h = max(params.close_height | 1, 1) - close_kernel = cv2.getStructuringElement( - cv2.MORPH_ELLIPSE, - (params.close_width, close_h), - ) - binary = cv2.morphologyEx( - binary, cv2.MORPH_CLOSE, close_kernel, - ) - - # 全ピクセルフィッティング region = binary[DETECT_Y_START:DETECT_Y_END, :] - ys_local, xs = np.where(region > 0) + centers_y: list[float] = [] + centers_x: list[float] = [] - if len(xs) < MIN_FIT_PIXELS: + 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) - ys = ys_local + DETECT_Y_START - coeffs = np.polyfit(ys, xs, 2) + cy = np.array(centers_y) + cx = np.array(centers_x) + + coeffs = clean_and_fit( + cy, cx, + median_ksize=median_ksize, + neighbor_thresh=neighbor_thresh, + residual_thresh=residual_thresh, + ransac_thresh=ransac_thresh, + ransac_iter=ransac_iter, + ) + if coeffs is None: + return no_detection(binary) + return build_result(coeffs, binary)