diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index bd4fd57..c5696eb 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -454,6 +454,14 @@ all_methods, ) + self._spin_residual_thresh = self._create_spin( + ip.residual_thresh, 0.0, 50.0, 1.0, + ) + self._add_image_row( + "残差除去:", self._spin_residual_thresh, + all_methods, + ) + # --- 案C/D: RANSAC --- self._spin_ransac_thresh = self._create_spin( ip.ransac_thresh, 1.0, 30.0, 1.0, @@ -684,6 +692,9 @@ self._spin_neighbor_thresh.setValue( ip.neighbor_thresh, ) + self._spin_residual_thresh.setValue( + ip.residual_thresh, + ) self._spin_ransac_thresh.setValue( ip.ransac_thresh, ) @@ -751,6 +762,9 @@ ip.neighbor_thresh = ( self._spin_neighbor_thresh.value() ) + ip.residual_thresh = ( + self._spin_residual_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 2433580..85d77d4 100644 --- a/src/pc/vision/line_detector.py +++ b/src/pc/vision/line_detector.py @@ -56,6 +56,7 @@ width_tolerance: 幅フィルタの上限倍率 median_ksize: 中心点列の移動メディアンフィルタサイズ(0 で無効) neighbor_thresh: 近傍外れ値除去の閾値(px,0 で無効) + residual_thresh: 残差反復除去の閾値(px,0 で無効) valley_gauss_ksize: 谷検出の行ごとガウシアンカーネルサイズ valley_min_depth: 谷として認識する最小深度 valley_max_deviation: 追跡予測からの最大許容偏差(px) @@ -97,6 +98,7 @@ # ロバストフィッティング(全手法共通) median_ksize: int = 7 neighbor_thresh: float = 10.0 + residual_thresh: float = 8.0 # 透視補正付き幅フィルタ(0 で無効) width_near: int = 0 @@ -265,6 +267,7 @@ binary, params.min_line_width, median_ksize=params.median_ksize, neighbor_thresh=params.neighbor_thresh, + residual_thresh=params.residual_thresh, ) @@ -318,6 +321,7 @@ binary, params.min_line_width, median_ksize=params.median_ksize, neighbor_thresh=params.neighbor_thresh, + residual_thresh=params.residual_thresh, ) @@ -370,6 +374,7 @@ ransac_iter=params.ransac_iter, median_ksize=params.median_ksize, neighbor_thresh=params.neighbor_thresh, + residual_thresh=params.residual_thresh, ) @@ -496,6 +501,7 @@ ransac_iter: int = 0, median_ksize: int = 0, neighbor_thresh: float = 0.0, + residual_thresh: float = 0.0, ) -> LineDetectResult: """行ごとの中心点に多項式をフィッティングする @@ -511,6 +517,7 @@ ransac_iter: RANSAC 反復回数 median_ksize: 移動メディアンのカーネルサイズ neighbor_thresh: 近傍外れ値除去の閾値 px + residual_thresh: 残差反復除去の閾値 px Returns: 線検出の結果 @@ -540,6 +547,7 @@ cy, cx, median_ksize=median_ksize, neighbor_thresh=neighbor_thresh, + residual_thresh=residual_thresh, ransac_thresh=ransac_thresh, ransac_iter=ransac_iter, ) @@ -603,6 +611,7 @@ cx: np.ndarray, median_ksize: int, neighbor_thresh: float, + residual_thresh: float = 0.0, weights: np.ndarray | None = None, ransac_thresh: float = 0.0, ransac_iter: int = 0, @@ -611,14 +620,16 @@ 全検出手法で共通に使えるロバストなフィッティング. (1) 移動メディアンフィルタでスパイクを平滑化 - (2) 近傍中央値からの偏差で外れ値を除去 + (2) 近傍中央値からの偏差で外れ値を除去(複数パス) (3) 重み付き最小二乗(または RANSAC)でフィッティング + (4) 残差ベースの反復除去で外れ値を最終除去 Args: cy: 中心点の y 座標配列 cx: 中心点の x 座標配列 median_ksize: 移動メディアンのカーネルサイズ(0 で無効) neighbor_thresh: 近傍外れ値除去の閾値 px(0 で無効) + residual_thresh: 残差除去の閾値 px(0 で無効) weights: 各点の信頼度(None で均等) ransac_thresh: RANSAC 閾値(0 以下で無効) ransac_iter: RANSAC 反復回数 @@ -641,15 +652,26 @@ hi = min(len(cx_clean), i + half + 1) cx_clean[i] = float(np.median(cx[lo:hi])) - # (2) 近傍外れ値除去 + # (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 + for _ in range(3): + new_mask = np.ones(len(cx_clean), dtype=bool) + for i in range(len(cx_clean)): + if not mask[i]: + continue + lo = max(0, i - half_n) + hi = min(len(cx_clean), i + half_n + 1) + neighbors = cx_clean[lo:hi][mask[lo:hi]] + if len(neighbors) == 0: + new_mask[i] = False + continue + local_med = float(np.median(neighbors)) + if abs(cx_clean[i] - local_med) > neighbor_thresh: + new_mask[i] = False + if np.array_equal(mask, mask & new_mask): + break + mask = mask & new_mask cy = cy[mask] cx_clean = cx_clean[mask] @@ -661,15 +683,39 @@ # (3) フィッティング if ransac_thresh > 0 and ransac_iter > 0: - return _ransac_polyfit( + 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 weights is not None: - return np.polyfit(cy, cx_clean, 2, w=weights) + if coeffs is None: + return None - return np.polyfit(cy, cx_clean, 2) + # (4) 残差ベースの反復除去 + if residual_thresh > 0: + for _ in range(5): + poly = np.poly1d(coeffs) + residuals = np.abs(cx_clean - poly(cy)) + inlier = residuals < residual_thresh + if np.all(inlier): + break + if np.sum(inlier) < MIN_FIT_ROWS: + break + cy = cy[inlier] + cx_clean = cx_clean[inlier] + if weights is not None: + weights = weights[inlier] + if weights is not None: + coeffs = np.polyfit( + cy, cx_clean, 2, w=weights, + ) + else: + coeffs = np.polyfit(cy, cx_clean, 2) + + return coeffs def _no_detection( @@ -1008,6 +1054,7 @@ 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,