#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
靶纸四角等腰直角三角形：检测、单应性落点、PnP 估距。
从 test/aruco_deteck.py 抽出，供主流程 shoot_manager 使用。
"""
import json
import os
import time
from itertools import combinations

import cv2
import numpy as np


def _log(msg):
    try:
        from logger_manager import logger_manager
        if logger_manager.logger:
            logger_manager.logger.info(msg)
    except Exception:
        pass


def _gray_suppress_bright_by_v(img_rgb, v_above: int):
    """
    RGB 输入：在 HSV 的 V 上，将亮度 >= v_above 的像素灰度置为 255。
    彩环（黄/红/蓝等）通常 V 较高，深色三角较低，可减轻「去黄掩码」类方案盖不全的问题，
    再交给现有 THRESH_BINARY_INV + Otsu 流程。
    """
    if img_rgb is None or img_rgb.size == 0:
        return cv2.cvtColor(img_rgb, cv2.COLOR_RGB2GRAY)
    va = int(np.clip(int(v_above), 0, 255))
    hsv = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2HSV)
    v = hsv[:, :, 2]
    gray = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2GRAY)
    out = gray.copy()
    out[v >= va] = 255
    return out


def _sharpen_triangle_gray(g_src, enable: bool, thresh: float, sigma: float, strength: float):
    """Unsharp mask；返回 (gray_out, laplacian_var, applied)。"""
    g = g_src.copy()
    lap_var = -1.0
    applied = False
    if not enable:
        return g, lap_var, applied
    try:
        if thresh > 0:
            lap_var = float(cv2.Laplacian(g, cv2.CV_64F).var())
            do_sh = lap_var < thresh
        else:
            do_sh = True
        if do_sh:
            blurred = cv2.GaussianBlur(g, (0, 0), float(sigma))
            w = float(strength)
            g = cv2.addWeighted(g, w, blurred, 1.0 - w, 0)
            applied = True
    except Exception:
        pass
    return g, lap_var, applied


def load_camera_from_xml(path):
    """读取 OpenCV FileStorage XML，返回 (camera_matrix, dist_coeffs) 或 (None, None)。"""
    if not path or not os.path.isfile(path):
        _log(f"[TRI] 标定文件不存在: {path}")
        return None, None
    try:
        fs = cv2.FileStorage(path, cv2.FILE_STORAGE_READ)
        K = fs.getNode("camera_matrix").mat()
        dist = fs.getNode("distortion_coefficients").mat()
        fs.release()
        if K is None or K.size == 0:
            return None, None
        if dist is None or dist.size == 0:
            dist = np.zeros((5, 1), dtype=np.float64)
        return K, dist
    except Exception as e:
        _log(f"[TRI] 读取标定失败: {e}")
        return None, None


def load_triangle_positions(path):
    """加载 triangle_positions.json，返回 dict[int, [x,y,z]]。"""
    if not path or not os.path.isfile(path):
        _log(f"[TRI] 三角形位置文件不存在: {path}")
        return None
    with open(path, "r", encoding="utf-8") as f:
        raw = json.load(f)
    return {int(k): v for k, v in raw.items()}


def homography_calibration(marker_centers, marker_ids, marker_positions, impact_point_pixel):
    target_points = []
    for mid in marker_ids:
        pos = marker_positions.get(mid)
        if pos is None:
            return False, None, None, None
        target_points.append([pos[0], pos[1]])

    src_pts = np.array(marker_centers, dtype=np.float32)
    dst_pts = np.array(target_points, dtype=np.float32)
    H, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, ransacReprojThreshold=1.0)
    if H is None:
        return False, None, None, None

    pt = np.array([[impact_point_pixel]], dtype=np.float32)
    transformed = cv2.perspectiveTransform(pt, H)
    target_x = float(transformed[0][0][0])
    target_y = float(transformed[0][0][1])
    return True, target_x, target_y, H


def complete_fourth_point(detected_ids, detected_centers, marker_positions):
    target_order = [0, 1, 2, 3]
    target_coords = {mid: marker_positions[mid][:2] for mid in target_order}
    all_ids = set(target_coords.keys())
    missing_id = (all_ids - set(detected_ids)).pop()

    known_src = []
    known_dst = []
    for mid, pt in zip(detected_ids, detected_centers):
        known_src.append(pt)
        known_dst.append(target_coords[mid])

    M_inv, _ = cv2.estimateAffine2D(
        np.array(known_dst, dtype=np.float32),
        np.array(known_src, dtype=np.float32),
    )
    if M_inv is None:
        return None

    missing_target = target_coords[missing_id]
    missing_src_h = M_inv @ np.array([missing_target[0], missing_target[1], 1.0])
    missing_src = missing_src_h[:2]

    complete_centers = []
    for mid in target_order:
        if mid == missing_id:
            complete_centers.append(missing_src)
        else:
            idx = detected_ids.index(mid)
            complete_centers.append(detected_centers[idx])

    return complete_centers, target_order


def pnp_distance_meters(marker_ids, marker_centers_px, marker_positions, K, dist):
    """
    靶面原点 (0,0,0) 到相机光心的距离：||tvec||，object 单位为 cm 时 tvec 为 cm，返回米。
    """
    obj = []
    for mid in marker_ids:
        p = marker_positions[mid]
        obj.append([float(p[0]), float(p[1]), float(p[2])])
    obj_pts = np.array(obj, dtype=np.float64)
    img_pts = np.array(marker_centers_px, dtype=np.float64)

    ok, rvec, tvec = cv2.solvePnP(
        obj_pts, img_pts, K, dist, flags=cv2.SOLVEPNP_ITERATIVE
    )
    if not ok:
        return None
    tvec = tvec.reshape(-1)
    dist_cm = float(np.linalg.norm(tvec))
    return dist_cm / 100.0


def detect_triangle_markers(
    gray_image,
    orig_gray=None,
    size_range=(8, 500),
    max_interior_gray=None,
    dark_pixel_gray=None,
    min_dark_ratio=None,
    verbose=True,
    skip_global_otsu_extract=False,
):
    # 读取可调参数（缺省值与 config.py 保持一致）
    try:
        import config as _cfg
        early_exit = int(getattr(_cfg, "TRIANGLE_EARLY_EXIT_CANDIDATES", 4))
        block_sizes = tuple(getattr(_cfg, "TRIANGLE_ADAPTIVE_BLOCK_SIZES", (11, 21, 35)))
        max_combo_n = int(getattr(_cfg, "TRIANGLE_MAX_FILTERED_FOR_COMBO", 10))
        if max_interior_gray is None:
            max_interior_gray = int(getattr(_cfg, "TRIANGLE_MAX_INTERIOR_GRAY", 130))
        if dark_pixel_gray is None:
            dark_pixel_gray = int(getattr(_cfg, "TRIANGLE_DARK_PIXEL_GRAY", 130))
        if min_dark_ratio is None:
            min_dark_ratio = float(getattr(_cfg, "TRIANGLE_MIN_DARK_RATIO", 0.30))
        min_contrast_diff = int(getattr(_cfg, "TRIANGLE_MIN_CONTRAST_DIFF", 15))
        shape_leg_tol = float(getattr(_cfg, "TRIANGLE_SHAPE_LEG_TOLERANCE", 0.25))
        shape_hyp_tol = float(getattr(_cfg, "TRIANGLE_SHAPE_HYP_TOLERANCE", 0.25))
        shape_cos_tol = float(getattr(_cfg, "TRIANGLE_SHAPE_COS_TOLERANCE", 0.25))
        roi_min_candidates = int(getattr(_cfg, "TRIANGLE_ROI_MIN_CANDIDATES", 3))
        multi_path_vote = bool(getattr(_cfg, "TRIANGLE_MULTI_PATH_VOTE", True))
        fallback_min_cand = int(getattr(_cfg, "TRIANGLE_FALLBACK_MIN_CANDIDATES", 3))
        relaxed_eps_enable = bool(getattr(_cfg, "TRIANGLE_FALLBACK_RELAXED_EPS", True))
        relaxed_eps_scale = float(getattr(_cfg, "TRIANGLE_RELAXED_POLY_EPS_SCALE", 1.65))
        blackhat_enable = bool(getattr(_cfg, "TRIANGLE_FALLBACK_BLACKHAT", True))
        blackhat_kernel_frac = float(getattr(_cfg, "TRIANGLE_BLACKHAT_KERNEL_FRAC", 0.018))
    except Exception:
        early_exit = 4
        block_sizes = (11, 21, 35)
        max_combo_n = 10
        if max_interior_gray is None:
            max_interior_gray = 130
        if dark_pixel_gray is None:
            dark_pixel_gray = 130
        if min_dark_ratio is None:
            min_dark_ratio = 0.30
        min_contrast_diff = 15
        shape_leg_tol = 0.25
        shape_hyp_tol = 0.25
        shape_cos_tol = 0.25
        roi_min_candidates = 3
        multi_path_vote = True
        fallback_min_cand = 3
        relaxed_eps_enable = True
        relaxed_eps_scale = 1.65
        blackhat_enable = True
        blackhat_kernel_frac = 0.018
    try:
        import config as _tcfg
        _timing_log = bool(getattr(_tcfg, "TRIANGLE_TIMING_LOG", True))
    except Exception:
        _timing_log = True

    _t_all = time.perf_counter()
    timings = {
        "global_otsu_ms": 0.0,
        "roi_ms": 0.0,
        "full_adaptive_ms": 0.0,
        "relax_ms": 0.0,
        "blackhat_ms": 0.0,
        "filter_combo_assign_ms": 0.0,
        "total_detect_ms": 0.0,
    }

    def _log_detect_timing():
        if not _timing_log:
            return
        timings["total_detect_ms"] = (time.perf_counter() - _t_all) * 1000
        _log(
            f"[TRI] timing_ms(detect): global_otsu={timings['global_otsu_ms']:.1f} "
            f"roi={timings['roi_ms']:.1f} full_adapt={timings['full_adaptive_ms']:.1f} "
            f"relax={timings['relax_ms']:.1f} blackhat={timings['blackhat_ms']:.1f} "
            f"filter_combo={timings['filter_combo_assign_ms']:.1f} "
            f"total_detect={timings['total_detect_ms']:.1f}"
        )

    min_leg, max_leg = size_range
    min_area = 0.5 * (min_leg ** 2) * 0.1
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
    if orig_gray is None:
        orig_gray = gray_image

    def _check_shape(approx):
        pts = approx.reshape(3, 2).astype(np.float32)
        sides = [
            np.linalg.norm(pts[1] - pts[0]),
            np.linalg.norm(pts[2] - pts[1]),
            np.linalg.norm(pts[0] - pts[2]),
        ]
        order = sorted(range(3), key=lambda i: sides[i])
        leg1, leg2, hyp = sides[order[0]], sides[order[1]], sides[order[2]]
        avg_leg = (leg1 + leg2) / 2

        if not (min_leg <= avg_leg <= max_leg):
            return None
        if abs(leg1 - leg2) / (avg_leg + 1e-6) > shape_leg_tol:
            return None
        if abs(hyp - avg_leg * np.sqrt(2)) / (avg_leg * np.sqrt(2) + 1e-6) > shape_hyp_tol:
            return None

        edge_verts = [(0, 1), (1, 2), (2, 0)]
        hv0, hv1 = edge_verts[order[2]]
        right_v = 3 - hv0 - hv1
        right_pt = pts[right_v]

        v0 = pts[hv0] - right_pt
        v1_vec = pts[hv1] - right_pt
        cos_a = np.dot(v0, v1_vec) / (
            np.linalg.norm(v0) * np.linalg.norm(v1_vec) + 1e-6
        )
        if abs(cos_a) > shape_cos_tol:
            return None

        return right_pt, avg_leg, pts

    def _color_ok(approx, gray_for_color):
        if gray_for_color is None:
            return True
        mask = np.zeros(gray_for_color.shape[:2], dtype=np.uint8)
        cv2.fillPoly(mask, [approx], 255)
        erode_k = max(1, int(min(gray_for_color.shape[:2]) * 0.002))
        erode_k = min(erode_k, 5)
        k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * erode_k + 1, 2 * erode_k + 1))
        mask_in = cv2.erode(mask, k, iterations=1)
        if cv2.countNonZero(mask_in) < 20:
            mask_in = mask

        mean_val = cv2.mean(gray_for_color, mask=mask_in)[0]
        ys, xs = np.where(mask_in > 0)
        if len(xs) == 0:
            return False
        interior = gray_for_color[ys, xs]
        dark_ratio = float(np.mean(interior <= dark_pixel_gray))

        # 条件1：绝对阈值（三角形内部足够暗）
        abs_ok = (mean_val <= max_interior_gray) and (dark_ratio >= min_dark_ratio)

        # 条件2：相对对比度 — 三角形内部比周围背景明显更暗
        contrast_ok = False
        if min_contrast_diff > 0:
            try:
                dilate_k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * erode_k + 3, 2 * erode_k + 3))
                mask_dilated = cv2.dilate(mask, dilate_k, iterations=2)
                mask_border = cv2.subtract(mask_dilated, mask)
                border_nz = cv2.countNonZero(mask_border)
                if border_nz > 20:
                    mean_surround = cv2.mean(gray_for_color, mask=mask_border)[0]
                    contrast_ok = (mean_surround - mean_val) >= min_contrast_diff
            except Exception:
                pass

        return abs_ok or contrast_ok

    def _extract_candidates(binary_img, gray_for_color, x_off=0, y_off=0, poly_eps_scale=1.0):
        contours, _ = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        found = []
        # ---- 诊断计数 ----
        _n_area_skip = 0
        _n_3vert = 0
        _n_shape_ok = 0
        _n_color_ok = 0
        _dbg_fail_shape = []   # 记录前几个失败原因
        _dbg_fail_color = []   # 记录前几个颜色失败详情
        pes = float(poly_eps_scale) if poly_eps_scale is not None else 1.0
        if pes < 0.5:
            pes = 0.5
        if pes > 3.0:
            pes = 3.0
        for cnt in contours:
            area = cv2.contourArea(cnt)
            if area < min_area:
                _n_area_skip += 1
                continue
            peri = cv2.arcLength(cnt, True)
            eps = (0.05 * peri if peri > 60 else 0.03 * peri) * pes
            approx = cv2.approxPolyDP(cnt, eps, True)
            if len(approx) != 3:
                continue
            _n_3vert += 1
            shape = _check_shape(approx)
            if shape is None:
                if len(_dbg_fail_shape) < 3:
                    pts3 = approx.reshape(3, 2).astype(np.float32)
                    sides = sorted([np.linalg.norm(pts3[1]-pts3[0]),
                                    np.linalg.norm(pts3[2]-pts3[1]),
                                    np.linalg.norm(pts3[0]-pts3[2])])
                    avg_l = (sides[0]+sides[1])/2
                    reason = f"avg_leg={avg_l:.1f} range=[{min_leg},{max_leg}] legs={sides[0]:.1f},{sides[1]:.1f} hyp={sides[2]:.1f} exp_hyp={avg_l*1.414:.1f}"
                    _dbg_fail_shape.append(reason)
                continue
            _n_shape_ok += 1
            if not _color_ok(approx, gray_for_color):
                if len(_dbg_fail_color) < 3 and gray_for_color is not None:
                    mask = np.zeros(gray_for_color.shape[:2], dtype=np.uint8)
                    cv2.fillPoly(mask, [approx], 255)
                    mean_v = cv2.mean(gray_for_color, mask=mask)[0]
                    ys, xs = np.where(mask > 0)
                    if len(xs) > 0:
                        dr = float(np.mean(gray_for_color[ys, xs] <= dark_pixel_gray))
                    else:
                        dr = 0
                    _dbg_fail_color.append(f"mean={mean_v:.1f}(<={max_interior_gray}?) dark_r={dr:.2f}(>={min_dark_ratio}?)")
                continue
            _n_color_ok += 1
            right_pt, avg_leg, pts = shape
            # 映射回全局坐标（ROI 会传 offset）
            pts_g = (pts + np.array([x_off, y_off], dtype=np.float32)).astype(np.float32)
            right_g = (right_pt + np.array([x_off, y_off], dtype=np.float32)).astype(np.float32)
            center_px = np.mean(pts_g, axis=0).tolist()
            dedup_key = f"{int(center_px[0] // 10)},{int(center_px[1] // 10)}"
            found.append({
                "center_px": center_px,
                "right_pt": right_g.tolist(),
                "corners": pts_g.tolist(),
                "avg_leg": avg_leg,
                "dedup_key": dedup_key,
            })
        if verbose:
            _log(f"[TRI] _extract: total={len(contours)} area_skip={_n_area_skip} "
                 f"3vert={_n_3vert} shape_ok={_n_shape_ok} color_ok={_n_color_ok}")
            if _dbg_fail_shape:
                _log(f"[TRI] shape失败原因(前3): {'; '.join(_dbg_fail_shape)}")
            if _dbg_fail_color:
                _log(f"[TRI] color失败原因(前3): {'; '.join(_dbg_fail_color)}")
        return found

    all_candidates = []
    seen_keys = set()
    # 便于日志确认本帧是否走了自适应 / ROI / 回退 / 多路径投票
    _tri_pipe = {
        "adaptive_runs": 0,
        "relaxed_eps": False,
        "roi": False,
        "blackhat": False,
        "roi_otsu_runs": 0,
        "roi_adaptive_runs": 0,
    }
    # 早退条件：不仅要数量够，还要候选分布足够分散（覆盖多个象限），避免误检集中导致提前退出
    h0, w0 = gray_image.shape[:2]
    cx0, cy0 = w0 / 2.0, h0 / 2.0
    seen_quadrants = set()
    # 4 个候选就够 4 角检测；3 个够 3 点补全，加 1 裕量
    _EARLY_EXIT = max(3, early_exit)

    def _add_from_binary(b, gray_for_color, x_off=0, y_off=0, poly_eps_scale=1.0):
        b = cv2.morphologyEx(b, cv2.MORPH_CLOSE, kernel)
        for c in _extract_candidates(
            b, gray_for_color, x_off=x_off, y_off=y_off, poly_eps_scale=poly_eps_scale
        ):
            key = c["dedup_key"]
            if key in seen_keys:
                if multi_path_vote:
                    for ex in all_candidates:
                        if ex["dedup_key"] == key:
                            ex["path_votes"] = int(ex.get("path_votes", 1)) + 1
                            break
                continue
            seen_keys.add(key)
            c["path_votes"] = 1
            all_candidates.append(c)
            # 象限统计：按图像中心划分
            tx, ty = c["center_px"]
            if tx < cx0 and ty < cy0:
                q = 0
            elif tx < cx0:
                q = 1
            elif ty >= cy0:
                q = 2
            else:
                q = 3
            seen_quadrants.add(q)

    def _should_early_exit():
        # 至少覆盖 3 个象限 + 数量达到阈值，才认为“足够像四角”可停止更多尝试
        return (len(all_candidates) >= _EARLY_EXIT) and (len(seen_quadrants) >= 3)

    # 1. 最快：全局 Otsu（无需逐像素邻域计算，~10ms）
    _t_seg = time.perf_counter()
    _, b_otsu = cv2.threshold(gray_image, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
    # ---- 临时调试：保存 Otsu 二值图供人工检查 ----
    try:
        import config as _dbg_cfg
        if getattr(_dbg_cfg, 'TRIANGLE_SAVE_DEBUG_IMAGE', False):
            _dbg_path = getattr(_dbg_cfg, 'PHOTO_DIR', '/root/phot') + '/tri_otsu_debug.jpg'
            cv2.imwrite(_dbg_path, b_otsu)
            _log(f"[TRI] DEBUG: Otsu 二值图已保存到 {_dbg_path}")
    except Exception:
        pass
    if not skip_global_otsu_extract:
        _add_from_binary(b_otsu, orig_gray, 0, 0)
    elif verbose:
        _log("[TRI] 跳过全局 Otsu 轮廓提取（仅从象限 ROI / adaptive / 回退提取）；b_otsu 仍保留")
    timings["global_otsu_ms"] = (time.perf_counter() - _t_seg) * 1000

    try:
        import config as _cfg
        roi_enabled = bool(getattr(_cfg, "TRIANGLE_ROI_ENABLED", False))
        roi_overlap = float(getattr(_cfg, "TRIANGLE_ROI_OVERLAP_RATIO", 0.08))
        roi_use_adapt = bool(getattr(_cfg, "TRIANGLE_ROI_USE_ADAPTIVE", True))
    except Exception:
        roi_enabled = False
        roi_overlap = 0.08
        roi_use_adapt = True

    # 2. ROI 局部阈值（提前）：象限各自阈值，优先于整图 adaptive；易早停则跳过最慢的整图 adaptive
    _t_seg = time.perf_counter()
    if (
        roi_enabled
        and (not _should_early_exit())
        and len(all_candidates) < roi_min_candidates
    ):
        _tri_pipe["roi"] = True
        h, w = gray_image.shape[:2]
        mx = int(max(0, min(w * roi_overlap, w * 0.25)))
        my = int(max(0, min(h * roi_overlap, h * 0.25)))
        mx = int(mx)
        my = int(my)
        midx = w // 2
        midy = h // 2
        rois = [
            (0, 0, min(w, midx + mx), min(h, midy + my)),                 # TL
            (max(0, midx - mx), 0, w, min(h, midy + my)),                 # TR
            (0, max(0, midy - my), min(w, midx + mx), h),                 # BL
            (max(0, midx - mx), max(0, midy - my), w, h),                 # BR
        ]
        if verbose:
            _log(f"[TRI] ROI局部阈值启用（先于整图adaptive）：overlap={roi_overlap:.2f}, rois={rois}")

        for i, (x0, y0, x1, y1) in enumerate(rois):
            if _should_early_exit():
                break
            if (x1 - x0) < 32 or (y1 - y0) < 32:
                continue
            g_roi = gray_image[y0:y1, x0:x1]
            og_roi = orig_gray[y0:y1, x0:x1] if orig_gray is not None else g_roi
            _, b_roi = cv2.threshold(g_roi, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
            _tri_pipe["roi_otsu_runs"] += 1
            _add_from_binary(b_roi, og_roi, x0, y0)
            if roi_use_adapt:
                for bs in block_sizes:
                    if _should_early_exit():
                        break
                    if bs is None:
                        continue
                    b = cv2.adaptiveThreshold(
                        g_roi, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY_INV, bs, 4
                    )
                    _tri_pipe["roi_adaptive_runs"] += 1
                    _add_from_binary(b, og_roi, x0, y0)
    if _tri_pipe["roi"]:
        timings["roi_ms"] = (time.perf_counter() - _t_seg) * 1000

    # 3. 整图自适应：ROI 后仍不足或未早停时；单帧成本高，依赖早停
    _t_seg = time.perf_counter()
    for block_size in block_sizes:
        if _should_early_exit():
            break
        if block_size is None:
            continue
        b = cv2.adaptiveThreshold(
            gray_image, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY_INV, block_size, 4
        )
        _tri_pipe["adaptive_runs"] += 1
        _add_from_binary(b, orig_gray, 0, 0)
    timings["full_adaptive_ms"] = (time.perf_counter() - _t_seg) * 1000

    # 3b. 失败回退：同一幅全局 Otsu 二值图用更宽松的 approxPolyDP
    _t_seg = time.perf_counter()
    if relaxed_eps_enable and len(all_candidates) < fallback_min_cand:
        _tri_pipe["relaxed_eps"] = True
        if verbose:
            _log(f"[TRI] 回退：放宽 approxPolyDP (scale={relaxed_eps_scale})")
        _add_from_binary(
            b_otsu, orig_gray, 0, 0, poly_eps_scale=relaxed_eps_scale
        )
    if _tri_pipe["relaxed_eps"]:
        timings["relax_ms"] = (time.perf_counter() - _t_seg) * 1000

    # 4. Black-hat + Otsu：仍不足时再试
    _t_seg = time.perf_counter()
    if blackhat_enable and len(all_candidates) < fallback_min_cand:
        _tri_pipe["blackhat"] = True
        h_bh, w_bh = gray_image.shape[:2]
        mh = max(7, int(min(h_bh, w_bh) * blackhat_kernel_frac))
        if mh % 2 == 0:
            mh += 1
        mh = min(mh, 31)
        k_bh = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (mh, mh))
        bh = cv2.morphologyEx(gray_image, cv2.MORPH_BLACKHAT, k_bh)
        _, b_bh = cv2.threshold(bh, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
        if verbose:
            _log(f"[TRI] 回退：BlackHat+Otsu (k={mh})")
        _add_from_binary(b_bh, orig_gray, 0, 0, poly_eps_scale=1.0)
        if relaxed_eps_enable and len(all_candidates) < fallback_min_cand:
            _add_from_binary(b_bh, orig_gray, 0, 0, poly_eps_scale=relaxed_eps_scale)
    if _tri_pipe["blackhat"]:
        timings["blackhat_ms"] = (time.perf_counter() - _t_seg) * 1000

    if verbose:
        vote_max = max((int(c.get("path_votes", 1)) for c in all_candidates), default=1)
        fusion_hits = sum(max(0, int(c.get("path_votes", 1)) - 1) for c in all_candidates)
        _log(
            f"[TRI] pipeline: otsu+adaptive×{_tri_pipe['adaptive_runs']}, "
            f"relax={_tri_pipe['relaxed_eps']}, roi={_tri_pipe['roi']}, "
            f"roi_otsu×{_tri_pipe['roi_otsu_runs']} roi_adapt×{_tri_pipe['roi_adaptive_runs']}, "
            f"blackhat={_tri_pipe['blackhat']}, vote_max={vote_max}, fusion_extra={fusion_hits}"
        )
        _log(f"[TRI] 候选三角形共 {len(all_candidates)} 个(预过滤前)")

    _t_post = time.perf_counter()
    if len(all_candidates) < 2:
        timings["filter_combo_assign_ms"] = 0.0
        _log_detect_timing()
        return []

    all_legs = [c["avg_leg"] for c in all_candidates]
    med_leg = float(np.median(all_legs))
    filtered = []
    for c in all_candidates:
        leg = c["avg_leg"]
        if med_leg > 1e-6 and not (0.40 * med_leg <= leg <= 2.0 * med_leg):
            continue
        filtered.append(c)

    if len(filtered) < 2:
        timings["filter_combo_assign_ms"] = (time.perf_counter() - _t_post) * 1000
        _log_detect_timing()
        return []

    # 候选过多时，四点组合枚举会变慢：截断到更可能的 max_combo_n 个候选
    if max_combo_n > 0 and len(filtered) > max_combo_n:
        # 多路径投票优先，其次 avg_leg 接近中位数（四角同尺度）
        med_leg = float(np.median([c["avg_leg"] for c in filtered]))
        filtered = sorted(
            filtered,
            key=lambda c: (
                -int(c.get("path_votes", 1)),
                abs(c["avg_leg"] - med_leg),
            ),
        )[:max_combo_n]

    def _order_quad(pts_4):
        by_y = sorted(range(4), key=lambda i: pts_4[i][1])
        top_pair = sorted(by_y[:2], key=lambda i: pts_4[i][0])
        bot_pair = sorted(by_y[2:], key=lambda i: pts_4[i][0])
        return top_pair[0], bot_pair[0], bot_pair[1], top_pair[1]

    def _score_quad(cands_4):
        pts = [np.array(c["center_px"]) for c in cands_4]
        legs = [c["avg_leg"] for c in cands_4]
        tl, bl, br, tr = _order_quad(pts)

        diag1 = np.linalg.norm(pts[tl] - pts[br])
        diag2 = np.linalg.norm(pts[bl] - pts[tr])
        diag_ratio = max(diag1, diag2) / (min(diag1, diag2) + 1e-6)

        s_top = np.linalg.norm(pts[tl] - pts[tr])
        s_bot = np.linalg.norm(pts[bl] - pts[br])
        s_left = np.linalg.norm(pts[tl] - pts[bl])
        s_right = np.linalg.norm(pts[tr] - pts[br])
        h_ratio = max(s_top, s_bot) / (min(s_top, s_bot) + 1e-6)
        v_ratio = max(s_left, s_right) / (min(s_left, s_right) + 1e-6)

        med_l = float(np.median(legs))
        leg_dev = max(abs(l - med_l) / (med_l + 1e-6) for l in legs)

        score = (diag_ratio - 1.0) * 3.0 + (h_ratio - 1.0) + (v_ratio - 1.0) + leg_dev * 2.0
        return score, (tl, bl, br, tr)

    assigned = None
    if len(filtered) >= 4:
        best_score = float("inf")
        best_combo = None
        best_order = None

        for combo in combinations(range(len(filtered)), 4):
            cands = [filtered[i] for i in combo]
            score, order = _score_quad(cands)
            if score < best_score:
                best_score = score
                best_combo = combo
                best_order = order

        if verbose:
            _log(f"[TRI] 最优四边形: score={best_score:.3f}")

        if best_score < 3.0:
            cands = [filtered[i] for i in best_combo]
            tl, bl, br, tr = best_order
            assigned = {
                0: cands[tl],
                1: cands[bl],
                2: cands[br],
                3: cands[tr],
            }

    if assigned is None:
        cx = np.mean([c["center_px"][0] for c in filtered])
        cy = np.mean([c["center_px"][1] for c in filtered])
        quadrant_map = {}
        for c in filtered:
            tx, ty = c["center_px"]
            if tx < cx and ty < cy:
                q = 0
            elif tx < cx:
                q = 1
            elif ty >= cy:
                q = 2
            else:
                q = 3
            if q not in quadrant_map or c["avg_leg"] > quadrant_map[q]["avg_leg"]:
                quadrant_map[q] = c
        assigned = quadrant_map

    result = []
    for tid in sorted(assigned.keys()):
        c = assigned[tid]
        result.append({
            "id": tid,
            "center": c["right_pt"],
            "corners": c["corners"],
        })
    timings["filter_combo_assign_ms"] = (time.perf_counter() - _t_post) * 1000
    _log_detect_timing()
    return result


def _median_leg_filter_yolo_candidates(all_candidates):
    if len(all_candidates) < 2:
        return []
    all_legs = [c["avg_leg"] for c in all_candidates]
    med_leg = float(np.median(all_legs))
    filtered = []
    for c in all_candidates:
        leg = c["avg_leg"]
        if med_leg > 1e-6 and not (0.40 * med_leg <= leg <= 2.0 * med_leg):
            continue
        filtered.append(c)
    if len(filtered) < 2:
        return []
    return filtered


def _tri_patch_check_shape(approx, min_leg, max_leg, leg_tol, hyp_tol, cos_tol):
    pts = approx.reshape(3, 2).astype(np.float32)
    sides = [
        np.linalg.norm(pts[1] - pts[0]),
        np.linalg.norm(pts[2] - pts[1]),
        np.linalg.norm(pts[0] - pts[2]),
    ]
    order = sorted(range(3), key=lambda i: sides[i])
    leg1, leg2, hyp = sides[order[0]], sides[order[1]], sides[order[2]]
    avg_leg = (leg1 + leg2) / 2
    if not (min_leg <= avg_leg <= max_leg):
        return None
    if abs(leg1 - leg2) / (avg_leg + 1e-6) > leg_tol:
        return None
    if abs(hyp - avg_leg * np.sqrt(2)) / (avg_leg * np.sqrt(2) + 1e-6) > hyp_tol:
        return None
    edge_verts = [(0, 1), (1, 2), (2, 0)]
    hv0, hv1 = edge_verts[order[2]]
    right_v = 3 - hv0 - hv1
    right_pt = pts[right_v]
    v0 = pts[hv0] - right_pt
    v1_vec = pts[hv1] - right_pt
    cos_a = np.dot(v0, v1_vec) / (np.linalg.norm(v0) * np.linalg.norm(v1_vec) + 1e-6)
    if abs(cos_a) > cos_tol:
        return None
    return right_pt, avg_leg, pts


def _tri_patch_color_ok(approx, gray_for_color, max_interior_gray, dark_pixel_gray, min_dark_ratio, min_contrast_diff):
    try:
        mask = np.zeros(gray_for_color.shape[:2], dtype=np.uint8)
        cv2.fillPoly(mask, [approx], 255)
        mean_val = float(cv2.mean(gray_for_color, mask=mask)[0])
        ys, xs = np.where(mask > 0)
        if len(xs) == 0:
            return False
        interior = gray_for_color[ys, xs]
        dark_ratio = float(np.mean(interior <= dark_pixel_gray))
        abs_ok = (mean_val <= max_interior_gray) and (dark_ratio >= min_dark_ratio)
        contrast_ok = False
        if min_contrast_diff > 0:
            erode_k = max(1, min(int(min(gray_for_color.shape[:2]) * 0.002), 5))
            k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * erode_k + 1, 2 * erode_k + 1))
            mask_in = cv2.erode(mask, k, iterations=1)
            dilate_k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * erode_k + 3, 2 * erode_k + 3))
            mask_dilated = cv2.dilate(mask, dilate_k, iterations=2)
            mask_border = cv2.subtract(mask_dilated, mask)
            if cv2.countNonZero(mask_border) > 20:
                mean_surround = cv2.mean(gray_for_color, mask=mask_border)[0]
                contrast_ok = (mean_surround - mean_val) >= min_contrast_diff
        return abs_ok or contrast_ok
    except Exception:
        return False


def _format_stage2_patch_binary_stats(st):
    """try_binary 返回的拒绝计数 → 简短可读字符串。"""
    if not st:
        return "—"
    return (
        f"轮廓{st['n_c']} 面积过小×{st['a_small']} 顶点≠3×{st['nv3']} "
        f"形状×{st['shape']} 颜色×{st['color']}"
    )


def _extract_triangle_from_yolo_patch(
    gray_patch,
    orig_patch,
    patch_tag="",
    do_reject_log=False,
):
    if gray_patch is None:
        if do_reject_log and patch_tag:
            _log(f"[TRI] Stage2子框{patch_tag}: 未检出 gray_patch=None")
        return None
    if gray_patch.size < 144:
        if do_reject_log and patch_tag:
            _log(
                f"[TRI] Stage2子框{patch_tag}: 未检出 gray过小 "
                f"(pixels={gray_patch.size}, 需≥144)"
            )
        return None
    try:
        import config as _cfg
        leg_tol = float(getattr(_cfg, "TRIANGLE_SHAPE_LEG_TOLERANCE", 0.25))
        hyp_tol = float(getattr(_cfg, "TRIANGLE_SHAPE_HYP_TOLERANCE", 0.25))
        cos_tol = float(getattr(_cfg, "TRIANGLE_SHAPE_COS_TOLERANCE", 0.25))
        max_interior_gray = int(getattr(_cfg, "TRIANGLE_MAX_INTERIOR_GRAY", 130))
        dark_pixel_gray = int(getattr(_cfg, "TRIANGLE_DARK_PIXEL_GRAY", 130))
        min_dark_ratio = float(getattr(_cfg, "TRIANGLE_MIN_DARK_RATIO", 0.30))
        min_contrast_diff = int(getattr(_cfg, "TRIANGLE_MIN_CONTRAST_DIFF", 15))
    except Exception:
        leg_tol = hyp_tol = cos_tol = 0.25
        max_interior_gray, dark_pixel_gray = 130, 130
        min_dark_ratio, min_contrast_diff = 0.30, 15

    try:
        import config as _pcfg
        _ap_frac = float(getattr(_pcfg, "TRIANGLE_PATCH_APPROXPOLY_FRAC", 0.04))
        _ap_mults = getattr(_pcfg, "TRIANGLE_PATCH_APPROXPOLY_RELAX_MULTS", (1.0, 1.35, 1.75))
        _blur_k = int(getattr(_pcfg, "TRIANGLE_PATCH_PRE_BLUR_KSIZE", 0))
    except Exception:
        _ap_frac, _ap_mults, _blur_k = 0.04, (1.0, 1.35, 1.75), 0
    if not (0.01 <= _ap_frac <= 0.2):
        _ap_frac = 0.04
    if not isinstance(_ap_mults, (list, tuple)) or len(_ap_mults) == 0:
        _ap_mults = (1.0, 1.35, 1.75)
    _ap_mults = tuple(float(x) for x in _ap_mults if float(x) > 0)
    if len(_ap_mults) == 0:
        _ap_mults = (1.0,)

    gray_for_bin = gray_patch
    if _blur_k >= 3 and _blur_k % 2 == 1:
        gray_for_bin = cv2.GaussianBlur(gray_patch, (_blur_k, _blur_k), 0)

    hp, wp = gray_patch.shape[:2]
    min_leg = max(4, int(min(hp, wp) * 0.05))
    max_leg = max(min_leg + 2, int(max(hp, wp) * 0.92))
    area_thr = float(min_leg * min_leg * 0.08)
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))

    def try_binary(binary, og):
        binary = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
        best_area = 0.0
        best = None
        st = {"n_c": 0, "a_small": 0, "nv3": 0, "shape": 0, "color": 0}
        contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        st["n_c"] = len(contours)
        for cnt in contours:
            area = float(cv2.contourArea(cnt))
            if area < area_thr:
                st["a_small"] += 1
                continue
            perim = cv2.arcLength(cnt, True)
            approx = None
            for _m in _ap_mults:
                approx = cv2.approxPolyDP(cnt, _ap_frac * float(_m) * perim, True)
                if len(approx) == 3:
                    break
            if approx is None or len(approx) != 3:
                st["nv3"] += 1
                continue
            shape = _tri_patch_check_shape(approx, min_leg, max_leg, leg_tol, hyp_tol, cos_tol)
            if shape is None:
                st["shape"] += 1
                continue
            right_pt, avg_leg, pts = shape
            if not _tri_patch_color_ok(
                approx, og, max_interior_gray, dark_pixel_gray, min_dark_ratio, min_contrast_diff
            ):
                st["color"] += 1
                continue
            if area > best_area:
                best_area = area
                corners = pts.reshape(3, 2).astype(np.float32)
                center_px = np.mean(corners, axis=0).tolist()
                best = {
                    "center_px": center_px,
                    "right_pt": right_pt.tolist(),
                    "corners": corners.tolist(),
                    "avg_leg": float(avg_leg),
                }
        return best, st

    _, b_otsu = cv2.threshold(gray_for_bin, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
    best, st_o = try_binary(b_otsu, orig_patch)
    st_a = None
    if best is None:
        bs = int(max(7, min(int(min(hp, wp) * 0.35) | 1, 51)))
        b2 = cv2.adaptiveThreshold(
            gray_for_bin,
            255,
            cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
            cv2.THRESH_BINARY_INV,
            bs,
            4,
        )
        best, st_a = try_binary(b2, orig_patch)
    if best is None and do_reject_log and patch_tag:
        _log(
            f"[TRI] Stage2子框{patch_tag}: 传统未检出 "
            f"patch={wp}×{hp}px min_leg={min_leg} max_leg={max_leg} area_thr≈{area_thr:.0f}px² "
            f"| Otsu[{_format_stage2_patch_binary_stats(st_o)}] "
            f"| Adpt[{_format_stage2_patch_binary_stats(st_a)}] "
            f"(shape=config TRIANGLE_SHAPE_* color=config TRIANGLE_MAX_INTERIOR_GRAY 等)"
        )
    return best


def _assign_marker_ids_from_filtered(filtered, verbose=True):
    """与 detect_triangle_markers 末尾一致：filtered 为含 center_px/right_pt/corners/avg_leg 的候选。"""
    if len(filtered) < 2:
        return []
    try:
        import config as _cfg
        max_combo_n = int(getattr(_cfg, "TRIANGLE_MAX_FILTERED_FOR_COMBO", 10))
    except Exception:
        max_combo_n = 10
    if max_combo_n > 0 and len(filtered) > max_combo_n:
        med_leg = float(np.median([c["avg_leg"] for c in filtered]))
        filtered = sorted(
            filtered,
            key=lambda c: (
                -int(c.get("path_votes", 1)),
                abs(c["avg_leg"] - med_leg),
            ),
        )[:max_combo_n]

    def _order_quad(pts_4):
        by_y = sorted(range(4), key=lambda i: pts_4[i][1])
        top_pair = sorted(by_y[:2], key=lambda i: pts_4[i][0])
        bot_pair = sorted(by_y[2:], key=lambda i: pts_4[i][0])
        return top_pair[0], bot_pair[0], bot_pair[1], top_pair[1]

    def _score_quad(cands_4):
        pts = [np.array(c["center_px"]) for c in cands_4]
        legs = [c["avg_leg"] for c in cands_4]
        tl, bl, br, tr = _order_quad(pts)
        diag1 = np.linalg.norm(pts[tl] - pts[br])
        diag2 = np.linalg.norm(pts[bl] - pts[tr])
        diag_ratio = max(diag1, diag2) / (min(diag1, diag2) + 1e-6)
        s_top = np.linalg.norm(pts[tl] - pts[tr])
        s_bot = np.linalg.norm(pts[bl] - pts[br])
        s_left = np.linalg.norm(pts[tl] - pts[bl])
        s_right = np.linalg.norm(pts[tr] - pts[br])
        h_ratio = max(s_top, s_bot) / (min(s_top, s_bot) + 1e-6)
        v_ratio = max(s_left, s_right) / (min(s_left, s_right) + 1e-6)
        med_l = float(np.median(legs))
        leg_dev = max(abs(l - med_l) / (med_l + 1e-6) for l in legs)
        score = (diag_ratio - 1.0) * 3.0 + (h_ratio - 1.0) + (v_ratio - 1.0) + leg_dev * 2.0
        return score, (tl, bl, br, tr)

    assigned = None
    if len(filtered) >= 4:
        best_score = float("inf")
        best_combo = None
        best_order = None
        for combo in combinations(range(len(filtered)), 4):
            cands = [filtered[i] for i in combo]
            score, order = _score_quad(cands)
            if score < best_score:
                best_score = score
                best_combo = combo
                best_order = order
        if verbose:
            _log(f"[TRI] YOLO子框四元组: score={best_score:.3f}")
        if best_score < 3.0 and best_combo is not None:
            cands = [filtered[i] for i in best_combo]
            tl, bl, br, tr = best_order
            assigned = {0: cands[tl], 1: cands[bl], 2: cands[br], 3: cands[tr]}

    if assigned is None:
        cx = np.mean([c["center_px"][0] for c in filtered])
        cy = np.mean([c["center_px"][1] for c in filtered])
        quadrant_map = {}
        for c in filtered:
            tx, ty = c["center_px"]
            if tx < cx and ty < cy:
                q = 0
            elif tx < cx:
                q = 1
            elif ty >= cy:
                q = 2
            else:
                q = 3
            if q not in quadrant_map or c["avg_leg"] > quadrant_map[q]["avg_leg"]:
                quadrant_map[q] = c
        assigned = quadrant_map

    result = []
    for tid in sorted(assigned.keys()):
        c = assigned[tid]
        result.append({
            "id": tid,
            "center": c["right_pt"],
            "corners": c["corners"],
        })
    return result


def _markers_from_black_yolo_patches(
    gray_det,
    orig_gray_det,
    black_boxes_work,
    w_work,
    h_work,
    det_scale,
    verbose=True,
):
    """
    在靶环 ROI 内、黑三角 YOLO 给出的每个子框上跑传统三角提取，再拼成与 detect_triangle_markers
    相同结构的 tri_markers（像素坐标须与 img_det 一致，以便后续 ×inv_scale 与第 4 点 ROI）。

    每个子框对应一个物理直角三角标记：`_extract_triangle_from_yolo_patch` 在每块里最多输出 **1 个**
    最佳三角形（按轮廓面积），不会在单框内要求 3 个。
    整幅靶纸需要 3～4 个角点，因此汇总后仍要求 **至少 3 个子框** 传统提取成功（否则回退整图 ROI）。
    """
    if not black_boxes_work:
        return []
    try:
        import config as _prcfg
        _patch_reject_log = bool(
            getattr(_prcfg, "TRIANGLE_LOG_STAGE2_PATCH_REJECT", False)
        )
    except Exception:
        _patch_reject_log = False
    gh, gw = gray_det.shape[:2]
    candidates = []
    for _pi, (bx0, by0, bx1, by1) in enumerate(black_boxes_work):
        xd0 = int(np.floor(float(bx0) * det_scale))
        yd0 = int(np.floor(float(by0) * det_scale))
        xd1 = int(np.ceil(float(bx1) * det_scale))
        yd1 = int(np.ceil(float(by1) * det_scale))
        xd0 = max(0, min(xd0, gw - 1))
        yd0 = max(0, min(yd0, gh - 1))
        xd1 = max(xd0 + 1, min(xd1, gw))
        yd1 = max(yd0 + 1, min(yd1, gh))
        _tag = (
            f"#{_pi} work=[{bx0},{by0},{bx1},{by1}] "
            f"gray_slice=[{xd0},{yd0},{xd1},{yd1}] box_to_gray_scale={det_scale:.4f}"
        )
        if xd1 - xd0 < 8 or yd1 - yd0 < 8:
            if _patch_reject_log:
                _log(
                    f"[TRI] Stage2子框{_tag}: 未检出 灰度切片边长<8px "
                    f"({xd1 - xd0}×{yd1 - yd0})"
                )
            continue
        gp = gray_det[yd0:yd1, xd0:xd1]
        og = orig_gray_det[yd0:yd1, xd0:xd1]
        one = _extract_triangle_from_yolo_patch(
            gp, og, patch_tag=_tag, do_reject_log=_patch_reject_log
        )
        if one is None:
            continue
        ox, oy = float(xd0), float(yd0)
        one["center_px"] = [one["center_px"][0] + ox, one["center_px"][1] + oy]
        one["right_pt"] = [one["right_pt"][0] + ox, one["right_pt"][1] + oy]
        one["corners"] = [[p[0] + ox, p[1] + oy] for p in one["corners"]]
        candidates.append(one)

    filt = _median_leg_filter_yolo_candidates(candidates)
    if len(filt) < 3:
        if verbose and black_boxes_work:
            _log(
                f"[TRI] 黑三角YOLO子框传统检测: YOLO子框={len(black_boxes_work)} "
                f"子框内传统成功={len(candidates)} "
                f"尺度过滤后={len(filt)}（median_leg∈[0.4,2.0]×中位数；"
                f"需≥3 个「传统成功」才不走整图 ROI；"
                f"成功数不足多半是 Otsu/形状/颜色校验未过，未必是 YOLO 框不准）"
            )
        return []
    markers = _assign_marker_ids_from_filtered(filt, verbose=verbose)
    if verbose and markers:
        _log(f"[TRI] 黑三角YOLO+子框传统: {len(markers)} 个角点（与 img_det 对齐的坐标）")
    return markers


def try_triangle_scoring(
    img_rgb,
    laser_xy,
    marker_positions,
    camera_matrix,
    dist_coeffs,
    size_range=(8, 500),
    roi_xyxy=None,
    black_yolo_boxes_work=None,
    yolo_ring_ms=0.0,
    yolo_black_ms=0.0,
):
    """
    尝试三角形单应性 + PnP 估距。
    img_rgb: RGB，与 laser_xy 同一像素坐标系（全图分辨率，与 K 标定一致）。
    roi_xyxy: 可选 (x0,y0,x1,y1) 全图上的裁剪框；若给定则先裁小图再检测三角形，
              角点坐标再平移回全图供单应性/PnP（例如 YOLO 靶环 ROI）。
    black_yolo_boxes_work: 可选，靶环**裁切图坐标**下的黑三角 YOLO 框列表；
              若配置启用且框数>0，则优先在各子框内跑传统检测，不足 3 个再回退整幅 ROI 流程。
    yolo_ring_ms / yolo_black_ms: shoot_manager.analyze_shot 内测量的两段 YOLO 耗时（毫秒），
              仅计入 timing 日志；本函数内不包含 NPU 推理时间。
    返回 dict:
      ok, dx_cm, dy_cm, distance_m, offset_method, distance_method
    """
    out = {
        "ok": False,
        "dx_cm": None,
        "dy_cm": None,
        "distance_m": None,
        "offset_method": None,
        "distance_method": None,
    }
    if marker_positions is None or camera_matrix is None or dist_coeffs is None:
        return out

    try:
        import config as _cfg_tl
        _try_timing_log = bool(getattr(_cfg_tl, "TRIANGLE_TIMING_LOG", True))
        _crop_min_side = int(getattr(_cfg_tl, "TRIANGLE_CROP_ROI_MIN_SIDE_PX", 64))
    except Exception:
        _try_timing_log = True
        _crop_min_side = 64
    _t_try0 = time.perf_counter()
    _t_seg = _t_try0
    prep_ms = 0.0
    detect1_ms = 0.0
    # detect1：Stage2 子框传统成功时为各子框内 Otsu/Adaptive…；整图回退时为 detect_triangle_markers
    detect1_source = ""
    clahe_ms = 0.0
    detect2_ms = 0.0
    geometry_ms = 0.0
    homography_ms = 0.0
    pnp_ms = 0.0

    try:
        _yr = float(yolo_ring_ms)
    except (TypeError, ValueError):
        _yr = 0.0
    try:
        _yb = float(yolo_black_ms)
    except (TypeError, ValueError):
        _yb = 0.0
    _tri_yolo_part = f"yolo_ring={_yr:.1f} yolo_black={_yb:.1f}"

    h_full, w_full = img_rgb.shape[:2]
    roi_origin_xy = (0, 0)
    img_work = img_rgb
    if roi_xyxy is not None:
        try:
            rx0, ry0, rx1, ry1 = [int(round(float(v))) for v in roi_xyxy]
            rx0 = max(0, min(rx0, w_full - 1))
            ry0 = max(0, min(ry0, h_full - 1))
            rx1 = max(rx0 + 1, min(rx1, w_full))
            ry1 = max(ry0 + 1, min(ry1, h_full))
            if (rx1 - rx0) >= _crop_min_side and (ry1 - ry0) >= _crop_min_side:
                img_work = img_rgb[ry0:ry1, rx0:rx1].copy()
                roi_origin_xy = (rx0, ry0)
                _log(
                    f"[TRI] 外接 ROI 裁剪: [{rx0},{ry0},{rx1},{ry1}] → "
                    f"{img_work.shape[1]}×{img_work.shape[0]}"
                )
        except Exception as _e_roi:
            _log(f"[TRI] roi_xyxy 无效，使用整图: {_e_roi}")
            img_work = img_rgb
            roi_origin_xy = (0, 0)

    h_orig, w_orig = img_work.shape[:2]

    # 缩图加速：嵌入式 CPU 上图像处理耗时与面积成正比。
    # 不再写死 320/640：默认按相机最长边缩到 1/2（由 config.TRIANGLE_DETECT_SCALE 控制）。
    # 检测完后把像素坐标乘以 inv_scale 还原到原始分辨率，再送入单应性/PnP（与 K 标定分辨率一致）
    try:
        import config as _cfg
        scale = float(getattr(_cfg, "TRIANGLE_DETECT_SCALE", 0.5))
    except Exception:
        scale = 0.5
    if not (0.05 <= scale <= 1.0):
        scale = 0.5
    MAX_DETECT_DIM = max(64, int(max(h_orig, w_orig) * scale))
    long_side = max(h_orig, w_orig)
    if long_side > MAX_DETECT_DIM:
        det_scale = MAX_DETECT_DIM / long_side
        det_w = int(w_orig * det_scale)
        det_h = int(h_orig * det_scale)
        img_det = cv2.resize(img_work, (det_w, det_h), interpolation=cv2.INTER_LINEAR)
        inv_scale = 1.0 / det_scale
        size_range_det = (max(4, int(size_range[0] * det_scale)),
                          max(8, int(size_range[1] * det_scale)))
    else:
        img_det = img_work
        inv_scale = 1.0
        det_scale = 1.0
        size_range_det = size_range

    gray_rgb = cv2.cvtColor(img_det, cv2.COLOR_RGB2GRAY)
    try:
        import config as _gcfg
        _gray_mode = str(getattr(_gcfg, "TRIANGLE_GRAY_MODE", "rgb")).lower().strip()
        _v_above = int(getattr(_gcfg, "TRIANGLE_HSV_V_SUPPRESS_ABOVE", 200))
    except Exception:
        _gray_mode = "rgb"
        _v_above = 200
    _v_above = max(0, min(255, _v_above))
    if _gray_mode not in ("rgb", "v_suppress", "fallback_v_suppress", "try_both"):
        _gray_mode = "rgb"

    gray_base = gray_rgb
    if _gray_mode == "v_suppress":
        gray_base = _gray_suppress_bright_by_v(img_det, _v_above)
        _log(
            f"[TRI] gray_mode=v_suppress: V>={_v_above} 的像素灰度→255（压制亮环，突出暗三角）"
        )

    # ── 轻量锐化（Unsharp Mask）：仅当清晰度低于阈值时才触发 ────────────────
    try:
        import config as _scfg
        _sharpen_enable = bool(getattr(_scfg, "TRIANGLE_SHARPEN_ENABLE", True))
        _sharpen_thresh = float(getattr(_scfg, "TRIANGLE_SHARPEN_THRESHOLD", 80.0))
        _sharpen_sigma = float(getattr(_scfg, "TRIANGLE_SHARPEN_SIGMA", 2.0))
        _sharpen_strength = float(getattr(_scfg, "TRIANGLE_SHARPEN_STRENGTH", 1.5))
    except Exception:
        _sharpen_enable = True
        _sharpen_thresh = 80.0
        _sharpen_sigma = 2.0
        _sharpen_strength = 1.5

    gray_a = gray_b = None
    if _gray_mode == "try_both":
        gray_a, _sharpness_val, _sharpen_applied = _sharpen_triangle_gray(
            gray_rgb.copy(),
            _sharpen_enable,
            _sharpen_thresh,
            _sharpen_sigma,
            _sharpen_strength,
        )
        gray_b, _, _ = _sharpen_triangle_gray(
            _gray_suppress_bright_by_v(img_det, _v_above),
            _sharpen_enable,
            _sharpen_thresh,
            _sharpen_sigma,
            _sharpen_strength,
        )
        gray = gray_a
        _gray_detect = gray_a
    else:
        gray, _sharpness_val, _sharpen_applied = _sharpen_triangle_gray(
            gray_base,
            _sharpen_enable,
            _sharpen_thresh,
            _sharpen_sigma,
            _sharpen_strength,
        )
        _gray_detect = gray

    if _try_timing_log:
        _log(
            f"[TRI] gray_mode={_gray_mode} sharpness={_sharpness_val:.1f} "
            f"sharpen={'yes' if _sharpen_applied else 'no'} "
            f"(thresh={_sharpen_thresh:.0f} sigma={_sharpen_sigma} strength={_sharpen_strength})"
        )

    prep_ms = (time.perf_counter() - _t_seg) * 1000

    skip_global_extract = False
    try:
        import config as _cfg_sg
        if bool(getattr(_cfg_sg, "TRIANGLE_SKIP_GLOBAL_OTSU_EXTRACT_ON_YOLO_ROI", False)):
            skip_global_extract = roi_xyxy is not None
    except Exception:
        pass

    tri_markers = []
    if black_yolo_boxes_work and len(black_yolo_boxes_work) > 0:
        try:
            import config as _patch_cfg
            _patch_gs = str(
                getattr(_patch_cfg, "TRIANGLE_BLACK_YOLO_PATCH_GRAY_SOURCE", "rgb")
            ).lower().strip()
        except Exception:
            _patch_gs = "rgb"
        if _patch_gs not in ("rgb", "global"):
            _patch_gs = "rgb"
        # Stage2 子框：在 Stage1 裁切 **未缩放** 灰度上裁剪（YOLO 框已是 work 坐标），
        # 避免 det 缩略图上 patch 仅十几个像素导致近似/形状全挂。输出再 ×det_scale 回到 img_det 系。
        _patch_markers_need_det_scale = False
        if det_scale < 1.0:
            _patch_markers_need_det_scale = True
            gray_rgb_w = cv2.cvtColor(img_work, cv2.COLOR_RGB2GRAY)
            if _patch_gs == "rgb":
                _gray_for_patches = gray_rgb_w
            elif _gray_mode == "try_both":
                _gray_for_patches, _, _ = _sharpen_triangle_gray(
                    gray_rgb_w.copy(),
                    _sharpen_enable,
                    _sharpen_thresh,
                    _sharpen_sigma,
                    _sharpen_strength,
                )
            else:
                gray_base_w = gray_rgb_w
                if _gray_mode == "v_suppress":
                    gray_base_w = _gray_suppress_bright_by_v(img_work, _v_above)
                _gray_for_patches, _, _ = _sharpen_triangle_gray(
                    gray_base_w,
                    _sharpen_enable,
                    _sharpen_thresh,
                    _sharpen_sigma,
                    _sharpen_strength,
                )
            _patch_coord_scale = 1.0
        else:
            _gray_for_patches = gray_rgb if _patch_gs == "rgb" else gray
            _patch_coord_scale = 1.0
        _log(
            f"[TRI] Stage2: 黑三角 YOLO 子框共 {len(black_yolo_boxes_work)} 个，"
            f"先在各子框内传统提取直角三角（每框最多 1 个）"
        )
        if _try_timing_log:
            if det_scale < 1.0:
                _log(
                    f"[TRI] Stage2 子框灰度: Stage1 全分辨率 {w_orig}×{h_orig}px "
                    f"(缩略图 det_scale={det_scale:.4f} 仅用于整图加速分支)；"
                    f"patch_gray_source={_patch_gs}"
                )
            elif _patch_gs == "rgb":
                _log(
                    "[TRI] Stage2 子框灰度: patch_gray_source=rgb（RGB→灰度，无锐化/V 抑制）；"
                    "漏检可改 TRIANGLE_BLACK_YOLO_PATCH_GRAY_SOURCE=global"
                )
            else:
                _log(
                    "[TRI] Stage2 子框灰度: patch_gray_source=global（与整幅 ROI 同一套 gray）"
                )
        _t_yb = time.perf_counter()
        tri_markers = _markers_from_black_yolo_patches(
            _gray_for_patches,
            _gray_for_patches,
            black_yolo_boxes_work,
            w_orig,
            h_orig,
            _patch_coord_scale,
            verbose=_try_timing_log,
        )
        if _patch_markers_need_det_scale and tri_markers:
            _ds = float(det_scale)
            for m in tri_markers:
                m["center"] = [m["center"][0] * _ds, m["center"][1] * _ds]
                _co = m.get("corners")
                if _co:
                    m["corners"] = [[c[0] * _ds, c[1] * _ds] for c in _co]
        detect1_ms = (time.perf_counter() - _t_yb) * 1000
        detect1_source = "stage2_subpatches"
        if len(tri_markers) >= 3:
            _log(
                f"[TRI] Stage2 子框传统成功: 已得 {len(tri_markers)} 个角点标记，"
                f"跳过整幅 Stage1 ROI 上的全局三角搜索"
            )

    try:
        import config as _fb_cfg
        _patch_fallback = bool(
            getattr(_fb_cfg, "TRIANGLE_BLACK_YOLO_FALLBACK_ON_PATCH_FAIL", True)
        )
    except Exception:
        _patch_fallback = True
    _run_full_triangle_detect = len(tri_markers) < 3
    if (
        not _patch_fallback
        and black_yolo_boxes_work
        and len(black_yolo_boxes_work) > 0
    ):
        _run_full_triangle_detect = False
    if (
        _run_full_triangle_detect
        and black_yolo_boxes_work
        and len(black_yolo_boxes_work) > 0
    ):
        _log(
            f"[TRI] 回退: Stage2 子框内传统未凑满 ≥3 个有效三角（或已关 FALLBACK 仅部分成功），"
            f"改为在整幅 Stage1 裁切图 {w_orig}×{h_orig}px 上跑 detect_triangle_markers"
        )
    if _run_full_triangle_detect:
        # 快速路径：直接在灰度图上跑（内部先走 Otsu，几乎不耗时）
        # try_both：rgb 与 V 压制各跑一遍 detect，取检出数更多的一侧（平局保留 rgb）
        detect1_source = "global_markers"
        _t_seg = time.perf_counter()
        if _gray_mode == "try_both":
            _t_rgb = time.perf_counter()
            ta = detect_triangle_markers(
                gray_a,
                orig_gray=gray_a,
                size_range=size_range_det,
                verbose=True,
                skip_global_otsu_extract=skip_global_extract,
            )
            _ms_rgb = (time.perf_counter() - _t_rgb) * 1000
            _t_vs = time.perf_counter()
            tb = detect_triangle_markers(
                gray_b,
                orig_gray=gray_b,
                size_range=size_range_det,
                verbose=True,
                skip_global_otsu_extract=skip_global_extract,
            )
            _ms_vsup = (time.perf_counter() - _t_vs) * 1000
            if len(tb) > len(ta):
                tri_markers = tb
                gray = gray_b
                _gray_detect = gray_b
                _log(
                    f"[TRI] try_both: 选用 v_suppress 检出 {len(tb)} 个（rgb={len(ta)}），"
                    f"V>={_v_above}"
                )
            else:
                tri_markers = ta
                gray = gray_a
                _gray_detect = gray_a
                _log(
                    f"[TRI] try_both: 选用 rgb 检出 {len(ta)} 个（v_suppress={len(tb)}），"
                    f"V>={_v_above}"
                )
            detect1_ms = (time.perf_counter() - _t_seg) * 1000
            _log(
                f"[TRI] try_both 耗时: rgb={_ms_rgb:.1f}ms, v_suppress={_ms_vsup:.1f}ms, "
                f"detect 合计={detect1_ms:.1f}ms（上方相邻两行 timing_ms(detect) 依次为 rgb → v_suppress）"
            )
        else:
            tri_markers = detect_triangle_markers(
                gray,
                orig_gray=gray,
                size_range=size_range_det,
                verbose=True,
                skip_global_otsu_extract=skip_global_extract,
            )
            detect1_ms = (time.perf_counter() - _t_seg) * 1000
            if _gray_mode == "fallback_v_suppress" and len(tri_markers) < 3:
                _t_fb = time.perf_counter()
                gray_fb, _, _ = _sharpen_triangle_gray(
                    _gray_suppress_bright_by_v(img_det, _v_above),
                    _sharpen_enable,
                    _sharpen_thresh,
                    _sharpen_sigma,
                    _sharpen_strength,
                )
                tri_markers = detect_triangle_markers(
                    gray_fb,
                    orig_gray=gray_fb,
                    size_range=size_range_det,
                    verbose=True,
                    skip_global_otsu_extract=skip_global_extract,
                )
                gray = gray_fb
                _gray_detect = gray_fb
                detect1_ms += (time.perf_counter() - _t_fb) * 1000
                _log(
                    f"[TRI] fallback_v_suppress: rgb 不足 3 个三角形，已用 V>={_v_above} 压制灰度重跑 "
                    f"→ {len(tri_markers)} 个"
                )

    if len(tri_markers) < 3:
        # 慢速兜底：CLAHE 增强对比度后再试（光线不均 / 局部过暗时有效）
        # 默认关闭以优先速度；由 config.TRIANGLE_ENABLE_CLAHE_FALLBACK 控制。
        try:
            import config as _cfg
            enable_clahe = bool(getattr(_cfg, "TRIANGLE_ENABLE_CLAHE_FALLBACK", False))
        except Exception:
            enable_clahe = False

        if enable_clahe:
            _log(f"[TRI] 快速路径不足{len(tri_markers)}个，启用CLAHE增强")
            _t_seg = time.perf_counter()
            clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
            gray_clahe = clahe.apply(gray)
            _gray_detect = gray_clahe
            clahe_ms = (time.perf_counter() - _t_seg) * 1000
            _t_seg = time.perf_counter()
            tri_markers = detect_triangle_markers(
                gray_clahe,
                orig_gray=gray,
                size_range=size_range_det,
                verbose=True,
                skip_global_otsu_extract=skip_global_extract,
            )
            detect2_ms = (time.perf_counter() - _t_seg) * 1000
        else:
            _log(f"[TRI] 快速路径不足{len(tri_markers)}个，跳过CLAHE兜底（已关闭）")

    if len(tri_markers) < 3:
        _log(f"[TRI] 三角形不足3个: {len(tri_markers)}")
        if _try_timing_log:
            _log(
                f"[TRI] timing_ms(try_triangle): {_tri_yolo_part} "
                f"prep={prep_ms:.1f} detect1={detect1_ms:.1f} "
                f"clahe={clahe_ms:.1f} detect2={detect2_ms:.1f} "
                f"total_try={(time.perf_counter() - _t_try0) * 1000:.1f} (失败提前返回)"
            )
        return out

    # ── 恰好 3 个时：可选在预测位置附近做小 ROI 搜索第 4 个真实三角 ────────────
    # 关闭时跳过后续整块逻辑，直接走 complete_fourth_point 虚拟第 4 点（config.TRIANGLE_FOURTH_ROI_SEARCH_ENABLE）。
    _fourth_roi_search = True
    try:
        import config as _f4cfg
        _fourth_roi_search = bool(getattr(_f4cfg, "TRIANGLE_FOURTH_ROI_SEARCH_ENABLE", True))
    except Exception:
        pass

    if len(tri_markers) == 3 and _fourth_roi_search:
        _t_roi4 = time.perf_counter()
        try:
            _pred_ids = [m["id"] for m in tri_markers]
            _pred_centers = [[float(m["center"][0]), float(m["center"][1])] for m in tri_markers]
            _pred_comp = complete_fourth_point(_pred_ids, _pred_centers, marker_positions)
            if _pred_comp is not None:
                _pred_all_centers, _pred_all_ids = _pred_comp
                _missing_id4 = int((set([0, 1, 2, 3]) - set(_pred_ids)).pop())
                _pred_pt = np.array(_pred_all_centers[_missing_id4], dtype=np.float32)

                # ROI 半径：marker 平均边长的 2.5 倍（至少 25px，最多 120px）
                _avg_leg_det = float(np.mean([m.get("avg_leg", 20) for m in tri_markers]))
                _roi_half = int(np.clip(_avg_leg_det * 2.5, 25, 120))

                _px, _py = int(round(_pred_pt[0])), int(round(_pred_pt[1]))
                _hd, _wd = _gray_detect.shape[:2]
                _rx1 = max(0, _px - _roi_half)
                _ry1 = max(0, _py - _roi_half)
                _rx2 = min(_wd, _px + _roi_half)
                _ry2 = min(_hd, _py + _roi_half)

                _found4 = None
                if _rx2 - _rx1 > 10 and _ry2 - _ry1 > 10:
                    _gray_roi4 = _gray_detect[_ry1:_ry2, _rx1:_rx2]
                    _kernel3 = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
                    _min_leg4, _max_leg4 = size_range_det

                    # 从 config 读形状容差和颜色阈值（与 detect_triangle_markers 保持一致）
                    try:
                        import config as _r4cfg
                        _r4_leg_tol = float(getattr(_r4cfg, "TRIANGLE_SHAPE_LEG_TOLERANCE", 0.25))
                        _r4_hyp_tol = float(getattr(_r4cfg, "TRIANGLE_SHAPE_HYP_TOLERANCE", 0.25))
                        _r4_cos_tol = float(getattr(_r4cfg, "TRIANGLE_SHAPE_COS_TOLERANCE", 0.25))
                        _r4_max_gray = int(getattr(_r4cfg, "TRIANGLE_MAX_INTERIOR_GRAY", 130))
                        _r4_dark_gray = int(getattr(_r4cfg, "TRIANGLE_DARK_PIXEL_GRAY", 130))
                        _r4_dark_ratio = float(getattr(_r4cfg, "TRIANGLE_MIN_DARK_RATIO", 0.30))
                        _r4_contrast = int(getattr(_r4cfg, "TRIANGLE_MIN_CONTRAST_DIFF", 15))
                    except Exception:
                        _r4_leg_tol, _r4_hyp_tol, _r4_cos_tol = 0.25, 0.25, 0.25
                        _r4_max_gray, _r4_dark_gray, _r4_dark_ratio, _r4_contrast = 130, 130, 0.30, 15

                    def _roi4_check_shape(approx):
                        """简化版等腰直角三角形形状检查（与 detect_triangle_markers._check_shape 逻辑一致）。"""
                        pts = approx.reshape(3, 2).astype(np.float32)
                        sides = [np.linalg.norm(pts[1]-pts[0]),
                                 np.linalg.norm(pts[2]-pts[1]),
                                 np.linalg.norm(pts[0]-pts[2])]
                        order = sorted(range(3), key=lambda i: sides[i])
                        leg1, leg2, hyp = sides[order[0]], sides[order[1]], sides[order[2]]
                        avg_leg = (leg1 + leg2) / 2
                        if not (_min_leg4 <= avg_leg <= _max_leg4):
                            return None
                        if abs(leg1 - leg2) / (avg_leg + 1e-6) > _r4_leg_tol:
                            return None
                        if abs(hyp - avg_leg * np.sqrt(2)) / (avg_leg * np.sqrt(2) + 1e-6) > _r4_hyp_tol:
                            return None
                        hv0, hv1 = [(0,1),(1,2),(2,0)][order[2]]
                        right_v = 3 - hv0 - hv1
                        right_pt = pts[right_v]
                        v0 = pts[hv0] - right_pt
                        v1v = pts[hv1] - right_pt
                        cos_a = np.dot(v0, v1v) / (np.linalg.norm(v0) * np.linalg.norm(v1v) + 1e-6)
                        if abs(cos_a) > _r4_cos_tol:
                            return None
                        return right_pt, avg_leg

                    def _roi4_color_ok(approx, gray_src):
                        """简化版颜色/对比度检查。"""
                        try:
                            mask = np.zeros(gray_src.shape[:2], dtype=np.uint8)
                            cv2.fillPoly(mask, [approx], 255)
                            mean_val = float(cv2.mean(gray_src, mask=mask)[0])
                            if mean_val > _r4_max_gray:
                                return False
                            dark_pixels = np.sum((gray_src < _r4_dark_gray) & (mask > 0))
                            total_pixels = np.sum(mask > 0)
                            if total_pixels < 3:
                                return True
                            if dark_pixels / total_pixels < _r4_dark_ratio:
                                return False
                            if _r4_contrast > 0:
                                erode_k = max(1, min(int(min(gray_src.shape[:2]) * 0.05), 5))
                                k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2*erode_k+1, 2*erode_k+1))
                                mask_e = cv2.erode(mask, k)
                                outer = cv2.dilate(mask, k) - mask
                                if np.sum(mask_e) > 0 and np.sum(outer) > 0:
                                    inner_mean = float(cv2.mean(gray_src, mask=mask_e)[0])
                                    outer_mean = float(cv2.mean(gray_src, mask=outer)[0])
                                    if outer_mean - inner_mean < _r4_contrast:
                                        return False
                        except Exception:
                            pass
                        return True

                    def _roi4_extract(binary_roi):
                        """在 ROI 二值图里找三角形轮廓，返回 (center, avg_leg, corners) 或 None。"""
                        _cnts, _ = cv2.findContours(binary_roi, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
                        _best_c, _best_dist = None, float("inf")
                        for _cnt in _cnts:
                            _approx = cv2.approxPolyDP(_cnt, 0.04 * cv2.arcLength(_cnt, True), True)
                            if len(_approx) != 3:
                                continue
                            _res = _roi4_check_shape(_approx)
                            if _res is None:
                                continue
                            _right_pt, _avg_leg = _res
                            if not _roi4_color_ok(_approx, _gray_roi4):
                                continue
                            _cx = float(_right_pt[0]) + _rx1
                            _cy = float(_right_pt[1]) + _ry1
                            _d = (_cx - _pred_pt[0]) ** 2 + (_cy - _pred_pt[1]) ** 2
                            if _d < _best_dist:
                                _best_dist = _d
                                _best_c = (_cx, _cy, _avg_leg,
                                           [[float(p[0][0]) + _rx1, float(p[0][1]) + _ry1]
                                            for p in _approx])
                        return _best_c

                    # 先试 Otsu
                    _, _bin4 = cv2.threshold(_gray_roi4, 0, 255,
                                             cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
                    _bin4 = cv2.morphologyEx(_bin4, cv2.MORPH_CLOSE, _kernel3)
                    _found4 = _roi4_extract(_bin4)

                    # Otsu 没找到时再试一次 adaptive（block_size 自适应 ROI 尺寸）
                    if _found4 is None:
                        _roi_short = min(_rx2 - _rx1, _ry2 - _ry1)
                        _blk = int(_roi_short * 0.4) | 1  # 取奇数，约为 ROI 边长 40%
                        _blk = max(7, min(_blk, 51))
                        _bin4a = cv2.adaptiveThreshold(
                            _gray_roi4, 255,
                            cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY_INV,
                            _blk, 4,
                        )
                        _bin4a = cv2.morphologyEx(_bin4a, cv2.MORPH_CLOSE, _kernel3)
                        _found4 = _roi4_extract(_bin4a)

                if _found4 is not None:
                    _cx4, _cy4, _al4, _co4 = _found4
                    _m4 = {"id": _missing_id4, "center": [_cx4, _cy4],
                           "corners": _co4, "avg_leg": _al4}
                    tri_markers.append(_m4)
                    _log(
                        f"[TRI] 4th-ROI 命中：预测({_px},{_py}) "
                        f"→ 实际({_cx4:.0f},{_cy4:.0f}) "
                        f"耗时{(time.perf_counter()-_t_roi4)*1000:.0f}ms"
                    )
                else:
                    _log(
                        f"[TRI] 4th-ROI 未命中：预测({_px},{_py}) "
                        f"roi=[{_rx1},{_ry1},{_rx2},{_ry2}] "
                        f"耗时{(time.perf_counter()-_t_roi4)*1000:.0f}ms，将用虚拟点"
                    )
        except Exception as _e4:
            _log(f"[TRI] 4th-ROI 搜索异常: {_e4}")
    elif len(tri_markers) == 3 and not _fourth_roi_search:
        _log("[TRI] 已跳过第4点 ROI 定向搜索(TRIANGLE_FOURTH_ROI_SEARCH_ENABLE=False)，将用几何虚拟第4点")

    # 将缩图坐标还原为裁剪图分辨率（再平移回全图，与 K / laser 一致）
    if inv_scale != 1.0:
        for m in tri_markers:
            m["center"] = [m["center"][0] * inv_scale, m["center"][1] * inv_scale]
            _co = m.get("corners")
            if _co:
                m["corners"] = [[c[0] * inv_scale, c[1] * inv_scale] for c in _co]

    ox, oy = roi_origin_xy
    if ox != 0 or oy != 0:
        for m in tri_markers:
            m["center"][0] += ox
            m["center"][1] += oy
            _co = m.get("corners")
            if _co:
                for c in _co:
                    c[0] += ox
                    c[1] += oy

    lx = float(np.clip(laser_xy[0], 0, w_full - 1))
    ly = float(np.clip(laser_xy[1], 0, h_full - 1))

    _t_seg = time.perf_counter()
    if len(tri_markers) == 4:
        tri_sorted = sorted(tri_markers, key=lambda m: m["id"])
        marker_ids = [m["id"] for m in tri_sorted]
        marker_centers = [[float(m["center"][0]), float(m["center"][1])] for m in tri_sorted]
        offset_tag = "triangle_homography"
    else:
        marker_ids_list = [m["id"] for m in tri_markers]
        marker_centers_orig = [[float(m["center"][0]), float(m["center"][1])] for m in tri_markers]
        comp = complete_fourth_point(marker_ids_list, marker_centers_orig, marker_positions)
        if comp is None:
            _log("[TRI] 3点补全第4点失败")
            if _try_timing_log:
                _log(
                    f"[TRI] timing_ms(try_triangle): {_tri_yolo_part} "
                    f"prep={prep_ms:.1f} detect1={detect1_ms:.1f} "
                    f"clahe={clahe_ms:.1f} detect2={detect2_ms:.1f} "
                    f"geometry={(time.perf_counter() - _t_seg) * 1000:.1f} "
                    f"total_try={(time.perf_counter() - _t_try0) * 1000:.1f} (3点补全失败)"
                )
            return out
        marker_centers, marker_ids = comp
        marker_centers = [[float(c[0]), float(c[1])] for c in marker_centers]
        offset_tag = "triangle_homography_3pt"

    # 供上层绘制：3点补全时，把补出来的第4点也返回（is_virtual=True）
    # 注意：markers_completed 与 out["markers"] 不同，后者仅包含真实检测到的三角形。
    try:
        _ids_detected = [int(m.get("id")) for m in tri_markers]
        _ids_all = {0, 1, 2, 3}
        _missing = list(_ids_all - set(_ids_detected))
        _missing_id = int(_missing[0]) if _missing else None
    except Exception:
        _missing_id = None

    _id2center = {int(i): [float(c[0]), float(c[1])] for i, c in zip(marker_ids, marker_centers)}
    _id2corners = {int(m["id"]): m.get("corners") for m in tri_markers if "id" in m}
    markers_completed = []
    for _tid in (0, 1, 2, 3):
        if _tid not in _id2center:
            continue
        _is_virtual = (offset_tag == "triangle_homography_3pt") and (_missing_id is not None) and (_tid == _missing_id)
        markers_completed.append({
            "id": _tid,
            "center": _id2center[_tid],
            "corners": _id2corners.get(_tid),
            "is_virtual": bool(_is_virtual),
        })

    # ---------- 结果有效性校验（防 nan/inf 与退化角点） ----------
    try:
        import config as _cfg
        min_center_dist_px = float(getattr(_cfg, "TRIANGLE_MIN_CENTER_DIST_PX", 3.0))
        max_dist_m = float(getattr(_cfg, "TRIANGLE_MAX_DISTANCE_M", 20.0))
    except Exception:
        min_center_dist_px = 3.0
        max_dist_m = 20.0

    def _all_finite(v) -> bool:
        try:
            return bool(np.all(np.isfinite(v)))
        except Exception:
            return False

    # 1) 4 个角点中心不能退化/重复（两两距离要大于阈值）
    try:
        pts = np.array(marker_centers, dtype=np.float64).reshape(-1, 2)
        ok_centers = True
        for i in range(len(pts)):
            for j in range(i + 1, len(pts)):
                if float(np.linalg.norm(pts[i] - pts[j])) <= min_center_dist_px:
                    ok_centers = False
                    break
            if not ok_centers:
                break
        if not ok_centers:
            _log(f"[TRI] 角点退化/重复：center_dist <= {min_center_dist_px:.1f}px，判定三角形失败")
            if _try_timing_log:
                _log(
                    f"[TRI] timing_ms(try_triangle): {_tri_yolo_part} "
                    f"prep={prep_ms:.1f} detect1={detect1_ms:.1f} "
                    f"clahe={clahe_ms:.1f} detect2={detect2_ms:.1f} "
                    f"geometry={(time.perf_counter() - _t_seg) * 1000:.1f} "
                    f"total_try={(time.perf_counter() - _t_try0) * 1000:.1f} (角点校验失败)"
                )
            return out
    except Exception:
        # 校验异常时不信任结果，直接回退
        _log("[TRI] 角点校验异常，判定三角形失败")
        if _try_timing_log:
            _log(
                f"[TRI] timing_ms(try_triangle): {_tri_yolo_part} "
                f"prep={prep_ms:.1f} detect1={detect1_ms:.1f} "
                f"geometry={(time.perf_counter() - _t_seg) * 1000:.1f} "
                f"total_try={(time.perf_counter() - _t_try0) * 1000:.1f} (校验异常)"
            )
        return out

    geometry_ms = (time.perf_counter() - _t_seg) * 1000
    _t_seg = time.perf_counter()
    ok_h, tx, ty, _H = homography_calibration(
        marker_centers, marker_ids, marker_positions, [lx, ly]
    )
    homography_ms = (time.perf_counter() - _t_seg) * 1000
    if not ok_h:
        _log("[TRI] 单应性失败")
        if _try_timing_log:
            _log(
                f"[TRI] timing_ms(try_triangle): {_tri_yolo_part} "
                f"prep={prep_ms:.1f} detect1={detect1_ms:.1f} "
                f"geometry={geometry_ms:.1f} homography={homography_ms:.1f} "
                f"total_try={(time.perf_counter() - _t_try0) * 1000:.1f} (单应性失败)"
            )
        return out

    # 2) 单应性矩阵必须是有限数
    if (not _all_finite(_H)):
        _log("[TRI] 单应性出现 nan/inf，判定三角形失败")
        if _try_timing_log:
            _log(
                f"[TRI] timing_ms(try_triangle): {_tri_yolo_part} "
                f"homography={homography_ms:.1f} "
                f"total_try={(time.perf_counter() - _t_try0) * 1000:.1f} (H 非有限)"
            )
        return out

    # 3) dx/dy 必须是有限数
    if (not _all_finite([tx, ty])):
        _log("[TRI] 偏移出现 nan/inf，判定三角形失败")
        if _try_timing_log:
            _log(
                f"[TRI] timing_ms(try_triangle): {_tri_yolo_part} "
                f"homography={homography_ms:.1f} "
                f"total_try={(time.perf_counter() - _t_try0) * 1000:.1f} (偏移非有限)"
            )
        return out

    # 与 laser_manager.compute_laser_position 现网约定一致：(x_cm, -y_cm_target)
    out["dx_cm"] = tx
    out["dy_cm"] = -ty
    out["ok"] = True
    out["offset_method"] = offset_tag
    out["markers"] = tri_markers   # 供上层绘制标注用
    out["markers_completed"] = markers_completed  # 含补全点（is_virtual=True）
    out["homography"] = _H         # 供上层反推靶心像素位置用

    _t_seg = time.perf_counter()
    dist_m = pnp_distance_meters(marker_ids, marker_centers, marker_positions, camera_matrix, dist_coeffs)
    pnp_ms = (time.perf_counter() - _t_seg) * 1000
    # 4) distance_m 若存在也必须是有限数且在合理范围（默认 <20m）
    if dist_m is not None and _all_finite([dist_m]) and 0.3 < dist_m < max_dist_m:
        out["distance_m"] = dist_m
        out["distance_method"] = "pnp_triangle"
        _log(f"[TRI] PnP 距离={dist_m:.2f}m, 偏移=({out['dx_cm']:.2f},{out['dy_cm']:.2f})cm")
    else:
        out["distance_m"] = None
        out["distance_method"] = None
        _log(f"[TRI] PnP 距离无效，回退黄心估距; 偏移=({out['dx_cm']:.2f},{out['dy_cm']:.2f})cm")

    if _try_timing_log:
        if detect1_source == "stage2_subpatches":
            _d1_note = (
                f"detect1=Stage2子框传统({len(black_yolo_boxes_work or [])}框逐块Otsu/Adaptive/轮廓，"
                f"非整图detect_triangle_markers)"
            )
        elif detect1_source == "global_markers":
            _d1_note = "detect1=整图detect_triangle_markers(或try_both双路)"
        else:
            _d1_note = "detect1=—"
        _log(
            f"[TRI] timing_ms(try_triangle): {_tri_yolo_part} "
            f"prep={prep_ms:.1f} detect1={detect1_ms:.1f} "
            f"clahe={clahe_ms:.1f} detect2={detect2_ms:.1f} geometry={geometry_ms:.1f} "
            f"homography={homography_ms:.1f} pnp={pnp_ms:.1f} "
            f"total_try={(time.perf_counter() - _t_try0) * 1000:.1f} "
            f"(不含 yolo_ring/yolo_black；完整箭内三角形链路见 [TRI] timing_e2e_triangle_ms)；{_d1_note}"
        )

    return out
