Newer
Older
RobotCar / src / pi / vision / detectors / dual_norm.py
"""
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,
    )