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 d462206..439af03 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" @@ -244,11 +244,14 @@ 7-2. アルゴリズム - (1) 多項式曲線上の2点の y 座標を決定する + (1) 二値画像の最大連結領域を抽出する + (2) 各行の左右端から中心 x 座標（row_centers）を求める + (3) 近い行と遠い行の y 座標を決定する near_y = 画像高さ × near_ratio（手前側） far_y = 画像高さ × far_ratio（奥側） - (2) 各点の x 座標を多項式から取得する - (3) 各点の偏差を計算する + (4) row_centers から直接 x 座標を取得する + （該当行が NaN の場合は近傍の有効な行を探索） + (5) 各点の偏差を計算する near_err = (画像中心x - near_x) / 画像中心x far_err = (画像中心x - far_x) / 画像中心x (4) 操舵量を算出する diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index 2725f0c..1516397 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -1210,6 +1210,8 @@ ("二値化画像", "binary"), ("検出領域", "detect_region"), ("フィッティング曲線", "poly_curve"), + ("行中心点", "row_centers"), + ("Theil-Sen直線", "theil_sen"), ("中心線", "center_line"), ] for label, attr in items: diff --git a/src/pc/steering/pursuit_control.py b/src/pc/steering/pursuit_control.py index a592972..71f8ce9 100644 --- a/src/pc/steering/pursuit_control.py +++ b/src/pc/steering/pursuit_control.py @@ -1,7 +1,7 @@ """ pursuit_control 2点パシュートによる操舵量計算モジュール -多項式曲線上の近距離・遠距離の2点から操舵量と速度を算出する +行中心点に Theil-Sen 直線近似を適用し，外れ値に強い操舵量を算出する """ from dataclasses import dataclass @@ -39,11 +39,42 @@ speed_k: float = 2.0 +def theil_sen_fit( + y: np.ndarray, + x: np.ndarray, +) -> tuple[float, float]: + """Theil-Sen 推定で直線 x = slope * y + intercept を求める + + 全ペアの傾きの中央値を使い，外れ値に強い直線近似を行う + + Args: + y: y 座標の配列（行番号） + x: x 座標の配列（各行の中心） + + Returns: + (slope, intercept) のタプル + """ + n = len(y) + slopes = [] + for i in range(n): + for j in range(i + 1, n): + dy = y[j] - y[i] + if dy != 0: + slopes.append((x[j] - x[i]) / dy) + + if len(slopes) == 0: + return 0.0, float(np.median(x)) + + slope = float(np.median(slopes)) + intercept = float(np.median(x - slope * y)) + return slope, intercept + + class PursuitControl(SteeringBase): """2点パシュートによる操舵量計算クラス - 多項式曲線上の近い点と遠い点の2箇所のx座標偏差から - 操舵量を計算する．微分・曲率の計算が不要で直感的 + 行中心点から Theil-Sen 直線近似を行い， + 直線上の近い点と遠い点の偏差から操舵量を計算する """ def __init__( @@ -77,22 +108,34 @@ result = detect_line(frame, self.image_params) self._last_result = result - if not result.detected or result.poly_coeffs is None: + if not result.detected or result.row_centers is None: return SteeringOutput( throttle=0.0, steer=0.0, ) - poly = np.poly1d(result.poly_coeffs) - center_x = config.FRAME_WIDTH / 2.0 - h = float(config.FRAME_HEIGHT) + centers = result.row_centers - # 2点の y 座標を計算 + # 有効な点（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 - - # 2点の x 座標を多項式から取得 - near_x = poly(near_y) - far_x = poly(far_y) + near_x = slope * near_y + intercept + far_x = slope * far_y + intercept # 各点の偏差（正: 線が左にある → 右に曲がる） near_err = (center_x - near_x) / center_x diff --git a/src/pc/vision/line_detector.py b/src/pc/vision/line_detector.py index c7a7efd..84ff7cd 100644 --- a/src/pc/vision/line_detector.py +++ b/src/pc/vision/line_detector.py @@ -133,6 +133,8 @@ heading: 線の傾き（dx/dy，画像下端での値） curvature: 線の曲率（d²x/dy²） poly_coeffs: 多項式の係数（描画用，未検出時は None） + row_centers: 各行の線中心 x 座標（index=行番号， + NaN=その行に線なし，未検出時は None） binary_image: 二値化後の画像（デバッグ用） """ @@ -141,6 +143,7 @@ heading: float curvature: float poly_coeffs: np.ndarray | None + row_centers: np.ndarray | None binary_image: np.ndarray | None @@ -786,6 +789,44 @@ return coeffs +def _extract_row_centers( + binary: np.ndarray, +) -> np.ndarray | None: + """二値画像の最大連結領域から各行の線中心を求める + + Args: + binary: 二値画像 + + Returns: + 各行の中心 x 座標（NaN=その行に線なし）， + 最大領域が見つからない場合は None + """ + h, w = binary.shape[:2] + num_labels, labels, stats, _ = ( + cv2.connectedComponentsWithStats(binary) + ) + + 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] + cols = np.where(row > 0)[0] + if len(cols) > 0: + centers[y] = (cols[0] + cols[-1]) / 2.0 + + return centers + + def _no_detection( binary: np.ndarray, ) -> LineDetectResult: @@ -796,6 +837,7 @@ heading=0.0, curvature=0.0, poly_coeffs=None, + row_centers=None, binary_image=binary, ) @@ -803,8 +845,12 @@ def _build_result( coeffs: np.ndarray, binary: np.ndarray, + row_centers: np.ndarray | None = None, ) -> LineDetectResult: - """多項式係数から LineDetectResult を構築する""" + """多項式係数から LineDetectResult を構築する + + row_centers が None の場合は binary から自動抽出する + """ poly = np.poly1d(coeffs) center_x = config.FRAME_WIDTH / 2.0 @@ -820,12 +866,17 @@ 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) + return LineDetectResult( detected=True, position_error=position_error, heading=heading, curvature=curvature, poly_coeffs=coeffs, + row_centers=row_centers, binary_image=binary, ) diff --git a/src/pc/vision/overlay.py b/src/pc/vision/overlay.py index 860e0f9..de4e2d6 100644 --- a/src/pc/vision/overlay.py +++ b/src/pc/vision/overlay.py @@ -15,6 +15,8 @@ COLOR_LINE: tuple = (0, 255, 0) COLOR_CENTER: tuple = (0, 255, 255) COLOR_REGION: tuple = (255, 0, 0) +COLOR_ROW_CENTER: tuple = (0, 165, 255) +COLOR_THEIL_SEN: tuple = (255, 0, 255) # 二値化オーバーレイの不透明度 BINARY_OPACITY: float = 0.4 @@ -28,11 +30,15 @@ binary: 二値化画像の半透明表示 detect_region: 検出領域の枠 poly_curve: フィッティング曲線 + row_centers: 各行の線中心点 + theil_sen: Theil-Sen 近似直線 center_line: 画像中心線 """ binary: bool = False detect_region: bool = False poly_curve: bool = False + row_centers: bool = False + theil_sen: bool = False center_line: bool = False @@ -86,6 +92,16 @@ if flags.poly_curve and result.poly_coeffs is not None: _draw_poly_curve(display, result.poly_coeffs) + # 各行の線中心点 + if flags.row_centers and result.row_centers is not None: + _draw_row_centers(display, result.row_centers) + + # Theil-Sen 近似直線 + if flags.theil_sen and result.row_centers is not None: + _draw_theil_sen_line( + display, result.row_centers, + ) + return display @@ -111,6 +127,68 @@ ) +def _draw_row_centers( + frame: np.ndarray, + centers: np.ndarray, +) -> None: + """各行の線中心点を描画する + + Args: + frame: 描画先の画像 + centers: 各行の中心 x 座標（NaN=線なし） + """ + w = frame.shape[1] + for y, cx in enumerate(centers): + if np.isnan(cx): + continue + ix = int(round(cx)) + if 0 <= ix < w: + frame[y, ix] = COLOR_ROW_CENTER + + +def _draw_theil_sen_line( + frame: np.ndarray, + centers: np.ndarray, +) -> None: + """行中心点から Theil-Sen 近似直線を描画する + + Args: + frame: 描画先の画像 + centers: 各行の中心 x 座標（NaN=線なし） + """ + h, w = frame.shape[:2] + valid = ~np.isnan(centers) + ys = np.where(valid)[0].astype(float) + xs = centers[valid] + + if len(ys) < 2: + return + + # Theil-Sen: 全ペアの傾きの中央値 + n = len(ys) + slopes = [] + for i in range(n): + for j in range(i + 1, n): + dy = ys[j] - ys[i] + if dy != 0: + slopes.append((xs[j] - xs[i]) / dy) + + if len(slopes) == 0: + return + + slope = float(np.median(slopes)) + intercept = float(np.median(xs - slope * ys)) + + # 直線の両端を計算して描画 + x0 = int(round(intercept)) + x1 = int(round(slope * (h - 1) + intercept)) + cv2.line( + frame, + (x0, 0), (x1, h - 1), + COLOR_THEIL_SEN, 1, + ) + + def _draw_poly_curve( frame: np.ndarray, coeffs: np.ndarray,