LSD-VP消失点検出

REM Python をシステム領域にインストール
winget install --scope machine --id Python.Python.3.12 -e --silent
REM Python のパス設定
set "PYTHON_PATH=C:\Program Files\Python312"
set "PYTHON_SCRIPTS_PATH=C:\Program Files\Python312\Scripts"
echo "%PATH%" | find /i "%PYTHON_PATH%" >nul
if errorlevel 1 setx PATH "%PATH%;%PYTHON_PATH%" /M >nul
echo "%PATH%" | find /i "%PYTHON_SCRIPTS_PATH%" >nul
if errorlevel 1 setx PATH "%PATH%;%PYTHON_SCRIPTS_PATH%" /M >nul

winget install --scope machine Codeium.Windsurf -e --silent

# LSD-VP消失点検出プログラム
#   特徴技術名: LSD (Line Segment Detector)
#   出典: Rafael Grompone von Gioi, Jérémie Jakubowicz, Jean-Michel Morel, and Gregory Randall, "LSD: a Line Segment Detector," Image Processing On Line, 2 (2012), pp. 35–55. https://doi.org/10.5201/ipol.2012.gjmr-lsd
#   特徴機能: パラメータ調整不要での自動線分検出 - 任意のデジタル画像に対してパラメータチューニングなしで動作し、線形時間でサブピクセル精度の線分検出を実現する機能。Helmholtz原理に基づいた誤検出制御により、平均して1画像につき1つの誤検出に制限。
#   学習済みモデル: 使用していない
#   方式設計:
#     関連利用技術:
#       - HDBSCAN: 階層密度ベースクラスタリング（Campello et al., 2013）- 異なる密度のクラスタを検出可能で、線分交点から消失点を自動検出
#       - OpenCV: コンピュータビジョンライブラリ - 画像処理とカメラ入力処理
#       - NumPy: 数値計算ライブラリ - 行列演算と幾何計算
#       - hdbscan: HDBSCANクラスタリング実装パッケージ
#       - Pillow: 日本語テキスト描画用ライブラリ
#     入力と出力:
#       - 入力: 動画（ユーザは「0:動画ファイル，1:カメラ，2:サンプル動画」のメニューで選択．0:動画ファイルの場合はtkinterでファイル選択．1の場合はOpenCVでカメラが開く．2の場合はhttps://raw.githubusercontent.com/opencv/opencv/master/samples/data/vtest.aviを使用）
#       - 出力: 処理結果が画像化できる場合にはOpenCV画面でリアルタイムに表示．OpenCV画面内に処理結果をテキストで表示．さらに，各フレームごとに，print()で処理結果を表示．プログラム終了時にprint()で表示した処理結果をresult.txtファイルに保存し，「result.txtに保存」したことをprint()で表示．プログラム開始時に，プログラムの概要，ユーザが行う必要がある操作（もしあれば）をprint()で表示．
#     処理手順:
#       1. LSDアルゴリズムによる線分検出
#       2. 線分の延長線の交点計算（画像外の交点も含む）
#       3. HDBSCANクラスタリングによる消失点推定
#       4. 結果の可視化
#     前処理: グレースケール変換による勾配計算最適化
#     後処理: 短い線分除去による精度向上
#     追加処理: 平行線分除去（角度差閾値による判定）- 消失点推定精度向上
#     調整を必要とする設定値:
#       - HDBSCAN_MIN_CLUSTER_SIZE（最小クラスタサイズ）: 消失点検出精度を左右する重要パラメータ
#   将来方策: HDBSCAN_MIN_CLUSTER_SIZEを画像解像度と検出された線分数に基づいて動的に調整する機能の実装。具体的には、画像面積と線分数の比率から最適なクラスタサイズを自動計算する
#   その他の重要事項: 建築物や道路の透視図法解析に特化、画像外の消失点も検出
#   前準備: pip install opencv-python opencv-contrib-python numpy hdbscan pillow

import cv2
import numpy as np
from hdbscan import HDBSCAN
import tkinter as tk
from tkinter import filedialog
import urllib.request
import time
from PIL import Image, ImageDraw, ImageFont
from datetime import datetime
import sys

# --- 設定 ---
BASE_HDBSCAN_MIN_CLUSTER_SIZE = 15  # ベースの最小クラスタサイズ（動的調整の基準）
HDBSCAN_MIN_SAMPLES = 5
INTERSECTION_THRESHOLD = 1e-10
VP_RADIUS = 15
MIN_LINE_LENGTH = 50
RANDOM_SEED = 42
# デフォルト角度閾値（ラジアン）
DEFAULT_ANGLE_THRESHOLD = 0.2
# デフォルト：消失点許容距離の倍率（画像対角長に対する倍率）
DEFAULT_MAX_VP_MULTIPLIER = 2.0
# 各消失点の最小支持線分数（ユニーク線分数の下限）
SUPPORT_MIN_LINES = 4

np.random.seed(RANDOM_SEED)

frame_count = 0
results_log = []

# 動的に HDBSCAN の min_cluster_size を計算する関数
def compute_dynamic_min_cluster_size(w, h, num_lines, base=BASE_HDBSCAN_MIN_CLUSTER_SIZE):
    # 基本方針: 画像面積と検出線分数に応じて min_cluster_size をスケールさせる
    # 参考の基準解像度は 1280x720
    ref_diag = np.sqrt(1280.0**2 + 720.0**2)
    diag = np.sqrt(w**2 + h**2)
    diag_scale = diag / ref_diag
    # 線分数の影響（少ないと小さめ、多いと大きめ）
    line_scale = max(1.0, num_lines / 50.0)
    size = int(max(3, base * diag_scale * line_scale))
    return size

# ユーザ設定の取得（角度閾値と距離倍率）
def ask_user_settings():
    try:
        inp = input(f"角度閾値（ラジアン, デフォルト {DEFAULT_ANGLE_THRESHOLD}）を入力（Enterでデフォルト）: ")
        angle_thresh = float(inp) if inp.strip() != "" else DEFAULT_ANGLE_THRESHOLD
    except Exception:
        angle_thresh = DEFAULT_ANGLE_THRESHOLD

    try:
        inp = input(f"消失点許容距離倍率（画像対角長の倍率, デフォルト {DEFAULT_MAX_VP_MULTIPLIER}）を入力（Enterでデフォルト）: ")
        max_vp_multiplier = float(inp) if inp.strip() != "" else DEFAULT_MAX_VP_MULTIPLIER
    except Exception:
        max_vp_multiplier = DEFAULT_MAX_VP_MULTIPLIER

    return angle_thresh, max_vp_multiplier

# 初期値（開始メッセージ表示後にユーザ入力で上書き）
ANGLE_THRESHOLD = DEFAULT_ANGLE_THRESHOLD
MAX_VP_MULTIPLIER = DEFAULT_MAX_VP_MULTIPLIER

# フレーム処理
def video_frame_processing(frame):
    global frame_count
    current_time = time.time()
    frame_count += 1

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    lsd = cv2.createLineSegmentDetector(0)
    lines = lsd.detect(gray)[0]

    vanishing_points = []
    vp_in_image = []
    vp_in_image_supports = []  # 画像内VPに対応する支持線分集合

    filtered_lines = []

    if lines is not None and len(lines) >= 2:
        # 短い線分を除外
        for line in lines:
            x1, y1, x2, y2 = line[0]
            length = np.hypot(x2 - x1, y2 - y1)
            if length >= MIN_LINE_LENGTH:
                filtered_lines.append(line)

        if len(filtered_lines) >= 2:
            h, w = frame.shape[:2]
            intersections = []
            intersection_pairs = []  # 交点を生成した線分ペア (i, j)

            for i in range(len(filtered_lines)):
                for j in range(i + 1, len(filtered_lines)):
                    x1, y1, x2, y2 = filtered_lines[i][0]
                    x3, y3, x4, y4 = filtered_lines[j][0]

                    v1 = np.array([x2 - x1, y2 - y1])
                    v2 = np.array([x4 - x3, y4 - y3])

                    angle1 = np.arctan2(v1[1], v1[0])
                    angle2 = np.arctan2(v2[1], v2[0])
                    angle_diff = abs(angle1 - angle2)
                    if angle_diff > np.pi:
                        angle_diff = 2 * np.pi - angle_diff

                    # 角度差が小さい（ほぼ平行）な場合は交点をスキップ
                    if angle_diff < ANGLE_THRESHOLD:
                        continue

                    denom = v1[0] * v2[1] - v1[1] * v2[0]
                    if abs(denom) < INTERSECTION_THRESHOLD:
                        continue

                    dx = x3 - x1
                    dy = y3 - y1
                    t = (dx * v2[1] - dy * v2[0]) / denom

                    x = x1 + t * v1[0]
                    y = y1 + t * v1[1]

                    center_x, center_y = w / 2, h / 2
                    distance = np.hypot(x - center_x, y - center_y)

                    # 画像対角長ベースで最大距離を計算
                    diag = np.hypot(w, h)
                    max_distance = MAX_VP_MULTIPLIER * diag

                    if distance < max_distance and np.isfinite(x) and np.isfinite(y):
                        intersections.append((x, y))
                        intersection_pairs.append((i, j))

            # HDBSCAN を実行（安全化＋対応付け＋ロバスト中心）
            if len(intersections) >= 2:
                points = np.array(intersections)

                # 動的に min_cluster_size を計算し、点数に応じて安全にクリップ
                dynamic_min_cluster = compute_dynamic_min_cluster_size(w, h, len(filtered_lines))
                n_points = len(points)
                min_cluster_size = max(2, min(dynamic_min_cluster, n_points))
                min_samples_val = max(1, min(HDBSCAN_MIN_SAMPLES, n_points - 1))

                clustering = HDBSCAN(min_cluster_size=min_cluster_size,
                                     min_samples=min_samples_val).fit(points)

                # ラベル列挙順を安定化（-1 はノイズとして除外）
                unique_labels = sorted(set(clustering.labels_) - {-1})

                for label in unique_labels:
                    idxs = np.where(clustering.labels_ == label)[0]
                    if len(idxs) == 0:
                        continue

                    cluster_points = points[idxs]

                    # クラスタの支持線分集合を構築（ユニーク線分数）
                    support_lines = set()
                    for idx in idxs:
                        li, lj = intersection_pairs[idx]
                        support_lines.add(li)
                        support_lines.add(lj)

                    # 最小支持線分数の下限でフィルタ
                    if len(support_lines) < SUPPORT_MIN_LINES:
                        continue

                    # メドイド（総距離最小の点）を中心として採用
                    m = len(cluster_points)
                    if m == 1:
                        vp = cluster_points[0]
                    else:
                        sums = []
                        for k in range(m):
                            diffs = cluster_points - cluster_points[k]
                            dists = np.hypot(diffs[:, 0], diffs[:, 1])
                            sums.append(np.sum(dists))
                        medoid_idx = int(np.argmin(sums))
                        vp = cluster_points[medoid_idx]

                    vanishing_points.append(vp)

                    if 0 <= vp[0] <= w and 0 <= vp[1] <= h:
                        vp_in_image.append(vp)
                        vp_in_image_supports.append(support_lines)

    # 結果画像作成
    result = frame.copy()

    # すべての検出線分を薄く描画（緑）
    if lines is not None:
        for line in lines:
            x1, y1, x2, y2 = map(int, line[0])
            cv2.line(result, (x1, y1), (x2, y2), (0, 200, 0), 1)

    # フィルタ後の線分を強調表示（薄青）
    for line in filtered_lines:
        x1, y1, x2, y2 = map(int, line[0])
        cv2.line(result, (x1, y1), (x2, y2), (200, 200, 0), 2)

    # 消失点を描画（円とラベル）
    colors = [(255, 0, 0), (0, 255, 0), (0, 0, 255)]
    for i, vp in enumerate(vp_in_image[:3]):
        x, y = int(vp[0]), int(vp[1])
        color = colors[i % len(colors)]
        cv2.circle(result, (x, y), VP_RADIUS, color, -1)
        cv2.putText(result, f'VP{i+1}', (x+20, y+5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)

    # 消失点から支持線分の中点へ線を描画（対応付け済みの線分のみ）
    for i, vp in enumerate(vp_in_image[:3]):
        color = colors[i % len(colors)]
        vx, vy = vp
        support_set = vp_in_image_supports[i] if i < len(vp_in_image_supports) else set()
        for line_idx in support_set:
            x1, y1, x2, y2 = filtered_lines[line_idx][0]
            mx, my = (x1 + x2) / 2.0, (y1 + y2) / 2.0
            cv2.line(result, (int(vx), int(vy)), (int(mx), int(my)), color, 1)

    # 日本語テキスト表示（全検出 / 使用）
    try:
        FONT_PATH = 'C:/Windows/Fonts/meiryo.ttc'
        FONT_SIZE = 20
        font = ImageFont.truetype(FONT_PATH, FONT_SIZE)
        img_pil = Image.fromarray(cv2.cvtColor(result, cv2.COLOR_BGR2RGB))
        draw = ImageDraw.Draw(img_pil)
        total_lines = len(lines) if lines is not None else 0
        used_lines = len(filtered_lines)
        draw.text((10, 30), f"線分数: {total_lines}（使用: {used_lines}）, 消失点: {len(vanishing_points)}（画像内: {len(vp_in_image)}）", font=font, fill=(255,255,255))
        result = cv2.cvtColor(np.array(img_pil), cv2.COLOR_RGB2BGR)
    except Exception:
        total_lines = len(lines) if lines is not None else 0
        used_lines = len(filtered_lines)
        cv2.putText(result, f"lines: {total_lines} (used: {used_lines}), VPs: {len(vanishing_points)} (in: {len(vp_in_image)})", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)

    # 結果文字列（座標と支持線分数は vp_in_image と対応）
    total_lines = len(lines) if lines is not None else 0
    result_str = f"消失点数: {len(vanishing_points)}個（画像内: {len(vp_in_image)}個）, 線分数: {total_lines} (使用: {len(filtered_lines)})"
    if vp_in_image:
        result_str += " 座標:"
        limit = min(3, len(vp_in_image))
        for i in range(limit):
            vp = vp_in_image[i]
            support_count = len(vp_in_image_supports[i]) if i < len(vp_in_image_supports) else 0
            result_str += f" VP{i+1}({vp[0]:.0f},{vp[1]:.0f},支持:{support_count})"

    return result, result_str, current_time

# --- 実行開始 ---
print("=== LSD-VP消失点検出プログラム（改善版） ===")
print("概要: 改善点を反映。消失点から線分への線描画や動的 HDBSCAN 設定などを実装。")
print("操作: q キーで終了")

# ユーザ設定の取得は開始メッセージ後に実施
ANGLE_THRESHOLD, MAX_VP_MULTIPLIER = ask_user_settings()

print("入力選択:\n 0: 動画ファイル\n 1: カメラ\n 2: サンプル動画")
choice = input("選択: ")

if choice == '0':
    root = tk.Tk()
    root.withdraw()
    path = filedialog.askopenfilename()
    if not path:
        print('ファイルが選択されませんでした。終了します。')
        sys.exit(0)
    cap = cv2.VideoCapture(path)
elif choice == '1':
    cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)
    if not cap.isOpened():
        cap = cv2.VideoCapture(0)
    cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
else:
    SAMPLE_URL = 'https://raw.githubusercontent.com/opencv/opencv/master/samples/data/vtest.avi'
    SAMPLE_FILE = 'vtest.avi'
    try:
        urllib.request.urlretrieve(SAMPLE_URL, SAMPLE_FILE)
    except Exception as e:
        print('サンプル動画のダウンロードに失敗しました:', e)
        sys.exit(1)
    cap = cv2.VideoCapture(SAMPLE_FILE)

if not cap.isOpened():
    print('動画ファイル・カメラを開けませんでした')
    sys.exit(1)

print('\n=== 動画処理開始 ===')
print('操作方法: q キー: プログラム終了')
try:
    while True:
        ret, frame = cap.read()
        if not ret:
            break

        MAIN_FUNC_DESC = "LSD-VP消失点検出（改善版）"
        processed_frame, result, current_time = video_frame_processing(frame)
        cv2.imshow(MAIN_FUNC_DESC, processed_frame)

        # print と result.txt の整合を確保（出力行をそのまま保存）
        if choice == '1':
            prefix = datetime.fromtimestamp(current_time).strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
        else:
            prefix = str(frame_count)
        log_line = f"{prefix} {result}"
        print(log_line)
        results_log.append(log_line)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
finally:
    print('\n=== プログラム終了 ===')
    cap.release()
    cv2.destroyAllWindows()
    if results_log:
        with open('result.txt', 'w', encoding='utf-8') as f:
            f.write('=== 結果 ===\n')
            f.write(f'処理フレーム数: {frame_count}\n')
            f.write('\n')
            f.write('\n'.join(results_log))
        print('\n処理結果をresult.txtに保存しました')