diff --git "a/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" "b/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" index 5c4bc20..23d8b3e 100644 --- "a/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" +++ "b/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" @@ -30,9 +30,9 @@ ・行中心点: 各行の線中心 x 座標をオレンジ色の点で描画 ・Theil-Sen 直線: 行中心点の Theil-Sen 近似直線をマゼンタで描画 ・中心線: 画像の中心 x に垂直線を描画(黄色) - ・パシュート目標点: 2点パシュートの near/far 目標点を赤色の - 円(半径 2px)で描画.制御手法がパシュート,かつ自動操縦中 - のみ有効 + ・パシュート目標点: 2点パシュートの near/far 目標点をシアンの + 円(半径 2px)で描画.制御手法がパシュートのとき有効 + (手動操作中は検出結果からプレビュー表示) 2-2. 検出情報ラベル(常時表示) @@ -46,7 +46,7 @@ ・検出領域: (255, 0, 0) 青 ・行中心点: (0, 165, 255) オレンジ ・Theil-Sen 直線: (255, 0, 255) マゼンタ - ・パシュート目標点: (0, 0, 255) 赤 + ・パシュート目標点: (255, 255, 0) シアン ・二値化オーバーレイ: 赤チャンネルに二値化画像を割り当て diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index 9efbacc..9509ee8 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -36,6 +36,7 @@ PursuitControl, PursuitParams, ) +from pc.vision.fitting import theil_sen_fit from pc.vision.line_detector import ( ImageParams, LineDetectResult, @@ -382,6 +383,39 @@ f" curv: {r.curvature:+.6f}" ) + def _calc_pursuit_points_preview( + self, + ) -> ( + tuple[tuple[float, float], tuple[float, float]] + | None + ): + """手動操作中にパシュート目標点を算出する + + Returns: + ((near_x, near_y), (far_x, far_y)) または None + """ + r = self._last_detect_result + if r is None or not r.detected: + return None + if r.row_centers is None: + return None + + centers = r.row_centers + valid = ~np.isnan(centers) + ys = np.where(valid)[0].astype(float) + xs = centers[valid] + if len(ys) < 2: + return None + + slope, intercept = theil_sen_fit(ys, xs) + h = len(centers) + p = self._pursuit_control.params + near_y = h * p.near_ratio + far_y = h * p.far_ratio + near_x = slope * near_y + intercept + far_x = slope * far_y + intercept + return ((near_x, near_y), (far_x, far_y)) + def _display_frame(self, frame: np.ndarray) -> None: """NumPy 配列の画像を QLabel に表示する @@ -394,9 +428,15 @@ # オーバーレイ描画 pursuit_pts = None if self._steering_method == "pursuit": - pursuit_pts = ( - self._pursuit_control.last_pursuit_points - ) + if self._is_auto: + pursuit_pts = ( + self._pursuit_control + .last_pursuit_points + ) + else: + pursuit_pts = ( + self._calc_pursuit_points_preview() + ) bgr = draw_overlay( bgr, self._last_detect_result, self._overlay_panel.get_flags(), diff --git a/src/pc/vision/overlay.py b/src/pc/vision/overlay.py index 44cbe87..f5bb0a5 100644 --- a/src/pc/vision/overlay.py +++ b/src/pc/vision/overlay.py @@ -18,7 +18,7 @@ COLOR_REGION: tuple = (255, 0, 0) COLOR_ROW_CENTER: tuple = (0, 165, 255) COLOR_THEIL_SEN: tuple = (255, 0, 255) -COLOR_PURSUIT: tuple = (0, 0, 255) +COLOR_PURSUIT: tuple = (255, 255, 0) # パシュート目標点の描画半径 PURSUIT_POINT_RADIUS: int = 2