Newer
Older
RobotCar / src / pc / vision / detectors / current.py
"""
current
現行手法: CLAHE + 固定閾値 + 全ピクセルフィッティング
"""

import cv2
import numpy as np

from pc.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)