你现在这个“看不清楚 / 右侧不是主视图 / 融合框和点云框一样”的根因其实主要有两点：

1. 右侧 FRONT 视图被你压成了总宽度的 10%（front_w 只有 0.2W），再加上点云用单像素画，缩放后几乎看不见。
2. 你做的是决策级融合：只融合类别/置信度，3D box7 直接沿用 PCDet 的框，所以 BEV/FRONT 的 3D 框必然和点云检测框一致（尺寸也不变）。你现在连 2D 投影框也直接用 pdet.proj_xyxy，所以 FUSED 在图上看起来和 PCDet 完全重叠。

下面我给你一份修改后的完整代码（全量代码），做到：

• 输出布局改成：左边=原图(含框)+BEV小窗+可选ZOOM小窗，右边=FRONT(X-Z) 主视图（右侧真正成为主视图，等宽显示，不再是细条）。
• BEV/FRONT 点云改为密度图渲染（bincount + log + gamma + dilate），远处稀疏点也能看清。
• FUSED 的 2D 显示框不再等于 PCDet 投影框：对匹配对，使用 YOLO 框 与 PCDet 投影框的加权融合（随匹配质量 q 变化），这样 FUSED 在图上能明显区分。
• BEV/FRONT 默认 只画 FUSED（否则 3D 框完全重合你肉眼看不出来）；想对比可打开开关。
• 保留你原来的 match quality / DS fusion / eval 打印 / 进度日志。

注意：默认仍然是决策级融合（3D box7 不变，所以“融合后 3D 框大小不变”是正常的）。如果你真的希望融合后 3D 框尺寸变化，需要 box-level/feature-level 的 3D 回归，这里我也给了一个可选启发式尺寸微调开关（默认关闭，慎用，会影响 3D IoU）。

python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
Fusion + Visualization + 3D Eval (NO intrusion logic)

改动要点（解决你说的“看不清楚 / 右侧不是主视图 / 融合框和点云框一样”）：
1) 可视化布局：输出改成【左：原图(含框) + BEV 小窗 + (可选)ZOOM 小窗】 | 【右：FRONT(X-Z) 主视图】，
   右侧 FRONT 直接等宽显示（不再是 10% 宽的小条），看点云/3D框更清楚。
2) 点云渲染：BEV / FRONT 改成“密度图”(bincount + log + gamma) 并做轻微膨胀，远处稀疏点也能看清。
3) 融合框为什么和点云框一样：
   - 你现在是“决策级融合”，只融合类别/置信度，不做 3D 框回归，所以 3D box7 必然沿用 PCDet 的框(大小不变)。
   - 但为了让你在图上能直观看到“融合框”不是一条线重叠，我们把 fused 的 2D 显示框改成
     YOLO 框与 PCDet 投影框的加权融合(随匹配质量 q 变化) —— 这样 FUSED 的 2D 框不再和 PCDet 一模一样。
   - 同时在 BEV/FRONT 里默认只画 FUSED（避免和 PCDet 3D 框完全重合看不出来），可用开关切回对比模式。
4) 其余：保持你原来的匹配/quality gating/eval 打印，并保留逐帧进度日志。

注意：本脚本默认仍是“决策级融合”（3D box7 不变），如果你真的希望融合后 3D 框尺寸变化，
需要 box-level/feature-level fusion 或额外的 3D 回归网络；这里仅提供一个可选的启发式尺寸微调开关（默认关闭）。
"""

import os
import sys
import glob
import math
import random
from dataclasses import dataclass
from pathlib import Path
from typing import Dict, List, Tuple, Optional

import numpy as np
import cv2
import torch
import torch.nn as nn


# =========================
# Paths (按你环境)
# =========================
IMG_DIR = "/media/jd/4997BB1603CFE2C4/lw/aug2/img"
PCDET_POINTS_DIR = "/media/jd/4997BB1603CFE2C4/lw/Openpcdet/data/custom/points"
PCDET_GT_LABEL_DIR = "/media/jd/4997BB1603CFE2C4/lw/Openpcdet/data/custom/labels"

ULTRALYTICS_REPO = "/media/jd/4997BB1603CFE2C4/lw/ultralytic"
YOLO_WEIGHTS = "/media/jd/4997BB1603CFE2C4/lw/ultralytic/runs1/1209yolo11_m+SM+d1/weights/best.pt"

OPENPCDET_REPO = "/media/jd/4997BB1603CFE2C4/lw/Openpcdet"
PCDET_CFG = "/media/jd/4997BB1603CFE2C4/lw/Openpcdet/tools/cfgs/custom_models/train.yaml"
PCDET_CKPT = "/media/jd/4997BB1603CFE2C4/lw/Openpcdet/output/cfgs/custom_models/train/default/ckpt/checkpoint_epoch_80.pth"

DEBUG_DIR = "debug_vis"


# =========================
# Classes
# =========================
CLASSES = ['Drone', 'Plastic_sheet', 'Kite', 'Balloon', 'Bird']


# =========================
# Calibration
# =========================
CALIB = {
    "extrinsic_matrix": np.array([
        [0.00871822977022152, -0.9990101808868351, -0.043619387365335945, -0.04000000000000002],
        [-0.0003806461322609286, 0.043617726472722454, -0.9990482215818578, 0.25],
        [0.9999619230641715, 0.008726535498373544, -1.3877787807814457e-17, -0.04000000000000002],
        [0.0, 0.0, 0.0, 1.0]
    ], dtype=np.float64),
    "fx": 3605.0801593073,
    "fy": 3604.9573399128,
    "cx": 951.9363889574,
    "cy": 816.9773743048,
    "width": 1920,
    "height": 1536,
    "dist": np.array([
        2.4515361243, -46.8007861419,
        -0.0002973913, -0.0008024507,
        -144.3698857610, 2.6420544359,
        -46.0443623397, -158.1742719597
    ], dtype=np.float64)
}

USE_DISTORTION = True
EXTRINSIC_MODE = "auto"  # auto / lidar2cam / cam2lidar


# =========================
# Colors (BGR)
# =========================
COL_YOLO = (0, 255, 0)
COL_PCDET = (255, 0, 0)
COL_FUSED = (0, 0, 255)


# =========================
# Thresholds
# =========================
YOLO_CONF_LOW = 0.05
PCDET_CONF_LOW = 0.01
YOLO_KEEP_TOPK = 1

PCDET_CONF_HIGH_UNMATCHED = 0.10
FUSED_KEEP_THRESH = 0.10

# matching
MATCH_IOU_THR = 0.05
MATCH_CENTER_DIST_THR_PX = 160.0
PROJ_BOX_EXPAND_RATIO = 0.18
COST_ALPHA = 0.7


# =========================
# Overlap visualization / scoring (稳定版)
# =========================
OV_IOU_THR = 0.20
OV_CENTER_DIST_PX = 40.0
OV_CONTAINMENT_THR = 0.60
OV_EDGE_GAP_PX = 6.0
OV_MIN_SCORE_FOR_STYLE = 0.55


# =========================
# Fusion: match quality -> affects confidence
# =========================
Q_IOU_REF = 0.30
Q_DIST_REF_PX = 80.0
Q_EDGE_REF_PX = 8.0
FUSED_Q_GATING_STRENGTH = 0.60


# =========================
# Debug / Eval
# =========================
RANDOM_SEED = 42

NUM_DEBUG_FRAMES = 20
NUM_EVAL_FRAMES = 800  # None=全量（慢），建议先用 300~1000 验证流程

EVAL_IOU_THRESHOLDS = [0.7]
EVAL_SCORE_THR_PCD = 0.10
EVAL_SCORE_THR_FUSED = 0.10

PROGRESS_EVERY = 20


# =========================
# BEV / FRONT rendering
# =========================
# 全局范围（当没有 det_ref 或你关闭 crop 时用）
BEV_X_RANGE = (0.0, 80.0)
BEV_Y_RANGE = (-30.0, 30.0)

# 更高分辨率 + crop（看局部更清楚）
BEV_RESOLUTION = 0.06
BEV_MIN_WPX = 360
BEV_MIN_HPX = 360
BEV_CROP_AROUND_DET = True
BEV_CROP_MARGIN_X = 22.0  # meters
BEV_CROP_MARGIN_Y = 22.0  # meters
BEV_DENSITY_GAMMA = 0.55
BEV_DILATE_KSIZE = 3

# FRONT(X-Z) “抠出来”视图
FRONT_RESOLUTION = 0.03
FRONT_MIN_WPX = 520   # 右侧主视图，最小宽度加大
FRONT_MIN_HPX = 520
FRONT_CROP_EXPAND_XY = 1.0
FRONT_CROP_EXPAND_Z = 0.8
FRONT_DENSITY_GAMMA = 0.55
FRONT_DILATE_KSIZE = 3

# =========================
# Visualization layout
# =========================
# 右侧作为主视图：只输出两栏 [IMAGE(with insets) | FRONT(main)]
VIS_LAYOUT = "front_main"   # front_main / classic_3panels（可自行扩展）
VIS_FRONT_WIDTH_RATIO_TO_IMG = 1.00   # FRONT 右侧宽度 = W * ratio
VIS_BEV_INSET_WIDTH_RATIO = 0.34      # BEV 作为左图的小窗，宽度占左图宽度比例
VIS_ADD_ZOOM_INSET = True
VIS_ZOOM_INSET_SIZE = 420             # px (square) in left image
VIS_ZOOM_CONTEXT_EXPAND_RATIO = 0.65  # 对框做扩张后裁剪（越大越包含背景）
VIS_OUT_SCALE = 1.0                   # 最终输出整体缩放（<1 可减小文件大小）

# 在 BEV/FRONT 视图里，如果有 fused_top，默认只画 FUSED（否则完全重叠看不出来）
VIS_SHOW_PCDET_WHEN_FUSED = False

# 融合 2D 显示框（让 FUSED 在图上不再和 PCDet 投影框完全一样）
FUSE_2D_BOX_FOR_VIS = True


# =========================
# Optional: heuristic 3D size refinement (默认关闭)
# =========================
# 注意：这不是严格的 3D box fusion，只是让 box7 尺寸随 2D 尺寸差异做一点点变化，
# 可能提高也可能降低 3D IoU；默认 False。
REFINE_3D_BOX_DIMS = False
REFINE_MAX_SCALE = 1.25
REFINE_MIN_SCALE = 0.75
REFINE_STRENGTH_XY = 0.25   # dx/dy 调整强度
REFINE_STRENGTH_Z = 0.20    # dz 调整强度


# =========================
# Sensor weights table (base for DS)
# =========================
PCDET_AP07 = {
    "Drone": 91.6,
    "Plastic_sheet": 55.52,
    "Kite": 40.61,
    "Balloon": 99.96,
    "Bird": 73.37
}


# =========================
# Data structures
# =========================
@dataclass
class Det2D:
    xyxy: List[float]
    cls_name: str
    score: float


@dataclass
class Det3D:
    box7: np.ndarray
    cls_name: str
    score: float
    proj_xyxy: Optional[List[float]] = None


# =========================
# Utils
# =========================
def ensure_dir(p: str):
    os.makedirs(p, exist_ok=True)


def canonical_class(name: str) -> str:
    if name is None:
        return name
    n = name.strip()
    n_low = n.lower().replace("-", "_")
    mapping = {
        "drone": "Drone",
        "kite": "Kite",
        "balloon": "Balloon",
        "bird": "Bird",
        "plastic_sheet": "Plastic_sheet",
        "plastic": "Plastic_sheet",
        "plasticsheet": "Plastic_sheet",
    }
    return mapping.get(n_low, n)


def find_image_for_frame(frame_id: str) -> Optional[str]:
    for ext in [".jpg", ".png", ".jpeg", ".bmp"]:
        p = os.path.join(IMG_DIR, frame_id + ext)
        if os.path.exists(p):
            return p
    g = glob.glob(os.path.join(IMG_DIR, frame_id + ".*"))
    return g[0] if g else None


def clip_box_xyxy(box, w, h):
    x1, y1, x2, y2 = box
    x1 = max(0, min(w - 1, x1))
    y1 = max(0, min(h - 1, y1))
    x2 = max(0, min(w - 1, x2))
    y2 = max(0, min(h - 1, y2))
    if x2 < x1:
        x1, x2 = x2, x1
    if y2 < y1:
        y1, y2 = y2, y1
    return [float(x1), float(y1), float(x2), float(y2)]


def expand_box_xyxy(box, ratio=0.1):
    x1, y1, x2, y2 = box
    cx = (x1 + x2) / 2.0
    cy = (y1 + y2) / 2.0
    w = max(1.0, (x2 - x1))
    h = max(1.0, (y2 - y1))
    w2 = w * (1.0 + ratio)
    h2 = h * (1.0 + ratio)
    return [cx - w2 / 2, cy - h2 / 2, cx + w2 / 2, cy + h2 / 2]


def box_iou2d(a, b) -> float:
    ax1, ay1, ax2, ay2 = a
    bx1, by1, bx2, by2 = b
    ix1 = max(ax1, bx1)
    iy1 = max(ay1, by1)
    ix2 = min(ax2, bx2)
    iy2 = min(ay2, by2)
    iw = max(0.0, ix2 - ix1)
    ih = max(0.0, iy2 - iy1)
    inter = iw * ih
    area_a = max(0.0, ax2 - ax1) * max(0.0, ay2 - ay1)
    area_b = max(0.0, bx2 - bx1) * max(0.0, by2 - by1)
    return float(inter / (area_a + area_b - inter + 1e-9))


def box_intersection_area(a, b) -> float:
    ax1, ay1, ax2, ay2 = a
    bx1, by1, bx2, by2 = b
    ix1 = max(ax1, bx1)
    iy1 = max(ay1, by1)
    ix2 = min(ax2, bx2)
    iy2 = min(ay2, by2)
    iw = max(0.0, ix2 - ix1)
    ih = max(0.0, iy2 - iy1)
    return float(iw * ih)


def box_area(a) -> float:
    return float(max(0.0, a[2] - a[0]) * max(0.0, a[3] - a[1]))


def center_of_box(box):
    x1, y1, x2, y2 = box
    return (0.5 * (x1 + x2), 0.5 * (y1 + y2))


def center_dist_px(a, b) -> float:
    ac = center_of_box(a)
    bc = center_of_box(b)
    return float(math.hypot(ac[0] - bc[0], ac[1] - bc[1]))


def point_in_box(pt, box):
    x, y = pt
    x1, y1, x2, y2 = box
    return (x >= x1) and (x <= x2) and (y >= y1) and (y <= y2)


def rect_edge_gap(a, b) -> float:
    """
    两个轴对齐矩形的边缘最近距离（0表示相交/重叠）
    """
    ax1, ay1, ax2, ay2 = a
    bx1, by1, bx2, by2 = b

    if ax2 < bx1:
        gx = bx1 - ax2
    elif bx2 < ax1:
        gx = ax1 - bx2
    else:
        gx = 0.0

    if ay2 < by1:
        gy = by1 - ay2
    elif by2 < ay1:
        gy = ay1 - by2
    else:
        gy = 0.0

    return float(math.hypot(gx, gy))


# =========================
# Stable overlap + match quality score
# =========================
def overlap_and_score_2d(a, b, diag: float) -> Tuple[bool, float, Dict]:
    iou = box_iou2d(a, b)
    dist = center_dist_px(a, b)
    gap = rect_edge_gap(a, b)

    inter = box_intersection_area(a, b)
    area_a = box_area(a)
    area_b = box_area(b)
    min_area = max(1e-9, min(area_a, area_b))
    containment = float(np.clip(inter / min_area, 0.0, 1.0))

    ac = center_of_box(a)
    bc = center_of_box(b)
    center_in = (point_in_box(ac, b) or point_in_box(bc, a))

    cond_iou = (iou >= OV_IOU_THR)
    cond_contain = (containment >= OV_CONTAINMENT_THR)
    cond_center_in = center_in
    cond_edge = (gap <= OV_EDGE_GAP_PX)
    cond_dist = (dist <= OV_CENTER_DIST_PX)
    is_overlap = cond_iou or cond_contain or cond_center_in or (cond_edge and cond_dist)

    iou_term = min(iou / Q_IOU_REF, 1.0)
    dist_term = max(0.0, 1.0 - dist / max(1e-6, Q_DIST_REF_PX))
    gap_term = max(0.0, 1.0 - gap / max(1e-6, Q_EDGE_REF_PX))
    contain_term = containment
    center_bonus = 0.15 if center_in else 0.0

    score = 0.45 * iou_term + 0.25 * dist_term + 0.20 * contain_term + 0.10 * gap_term + center_bonus
    score = float(np.clip(score, 0.0, 1.0))

    info = {"iou": iou, "dist": dist, "gap": gap, "containment": containment, "center_in": center_in}
    return is_overlap, score, info


def match_quality_2d(yolo_box, proj_box, img_w, img_h) -> Tuple[float, Dict]:
    diag = math.hypot(img_w, img_h) + 1e-9
    _, score, info = overlap_and_score_2d(yolo_box, proj_box, diag)
    q = float(np.clip(0.15 + 0.85 * score, 0.0, 1.0))
    return q, info


# =========================
# Drawing helpers
# =========================
def draw_text_box(img, x, y, text, color, font_scale=0.6, thickness=2):
    (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, font_scale, thickness)
    x = int(np.clip(x, 0, img.shape[1] - 1))
    y = int(np.clip(y, 0, img.shape[0] - 1))
    x2 = int(np.clip(x + tw + 6, 0, img.shape[1] - 1))
    y2 = int(np.clip(y + th + 6, 0, img.shape[0] - 1))
    cv2.rectangle(img, (x, y), (x2, y2), (0, 0, 0), -1)
    cv2.putText(img, text, (x + 3, y + th + 3), cv2.FONT_HERSHEY_SIMPLEX, font_scale, color, thickness, cv2.LINE_AA)


def draw_transparent_rect(img, box, color, alpha=0.22):
    x1, y1, x2, y2 = [int(round(v)) for v in box]
    overlay = img.copy()
    cv2.rectangle(overlay, (x1, y1), (x2, y2), color, -1)
    cv2.addWeighted(overlay, alpha, img, 1.0 - alpha, 0, img)


def draw_dashed_line(img, p1, p2, color, thickness=2, dash_len=14, gap_len=8):
    x1, y1 = p1
    x2, y2 = p2
    dx = x2 - x1
    dy = y2 - y1
    length = math.hypot(dx, dy)
    if length < 1e-6:
        return
    vx = dx / length
    vy = dy / length
    dist = 0.0
    while dist < length:
        seg_start = dist
        seg_end = min(dist + dash_len, length)
        sx = int(round(x1 + vx * seg_start))
        sy = int(round(y1 + vy * seg_start))
        ex = int(round(x1 + vx * seg_end))
        ey = int(round(y1 + vy * seg_end))
        cv2.line(img, (sx, sy), (ex, ey), color, thickness, cv2.LINE_AA)
        dist += dash_len + gap_len


def draw_dashed_rect(img, box, color, thickness=2):
    x1, y1, x2, y2 = [int(round(v)) for v in box]
    draw_dashed_line(img, (x1, y1), (x2, y1), color, thickness)
    draw_dashed_line(img, (x2, y1), (x2, y2), color, thickness)
    draw_dashed_line(img, (x2, y2), (x1, y2), color, thickness)
    draw_dashed_line(img, (x1, y2), (x1, y1), color, thickness)


def draw_box_smart(img, box, color, overlap_score: float, solid_thickness=2):
    if overlap_score >= OV_MIN_SCORE_FOR_STYLE:
        draw_transparent_rect(img, box, color, alpha=0.22)
        draw_dashed_rect(img, box, color, thickness=max(2, solid_thickness))
    else:
        x1, y1, x2, y2 = [int(round(v)) for v in box]
        cv2.rectangle(img, (x1, y1), (x2, y2), color, solid_thickness)


def draw_legend(img, x=12, y=12):
    draw_text_box(img, x, y, "Legend:", (255, 255, 255), 0.65, 2)
    y += 30
    cv2.rectangle(img, (x, y + 6), (x + 18, y + 24), COL_YOLO, -1)
    draw_text_box(img, x + 26, y, "YOLO (2D)", (255, 255, 255), 0.6, 2)
    y += 28
    cv2.rectangle(img, (x, y + 6), (x + 18, y + 24), COL_PCDET, -1)
    draw_text_box(img, x + 26, y, "PCDet-proj (3D->2D)", (255, 255, 255), 0.6, 2)
    y += 28
    cv2.rectangle(img, (x, y + 6), (x + 18, y + 24), COL_FUSED, -1)
    draw_text_box(img, x + 26, y, "FUSED", (255, 255, 255), 0.6, 2)
    y += 30
    draw_text_box(img, x, y, "Overlap: IoU/dist/contain/edge/center-in", (255, 255, 255), 0.50, 2)


def resize_keep_aspect(img: np.ndarray,
                       target_w: Optional[int] = None,
                       target_h: Optional[int] = None,
                       interp=cv2.INTER_AREA) -> np.ndarray:
    h, w = img.shape[:2]
    if target_w is None and target_h is None:
        return img
    if target_w is not None and target_h is not None:
        scale = min(target_w / max(1, w), target_h / max(1, h))
    elif target_w is not None:
        scale = target_w / max(1, w)
    else:
        scale = target_h / max(1, h)
    nw = max(1, int(round(w * scale)))
    nh = max(1, int(round(h * scale)))
    if nw == w and nh == h:
        return img
    return cv2.resize(img, (nw, nh), interpolation=interp)


def fit_to_rect(img: np.ndarray,
                target_w: int,
                target_h: int,
                pad_color=(0, 0, 0),
                interp=cv2.INTER_AREA) -> np.ndarray:
    resized = resize_keep_aspect(img, target_w=target_w, target_h=target_h, interp=interp)
    h, w = resized.shape[:2]
    out = np.zeros((target_h, target_w, 3), dtype=np.uint8)
    out[:] = pad_color
    y0 = (target_h - h) // 2
    x0 = (target_w - w) // 2
    out[y0:y0 + h, x0:x0 + w] = resized
    return out


def paste_inset(base: np.ndarray,
                inset: np.ndarray,
                x: int,
                y: int,
                border: int = 2,
                border_color=(255, 255, 255)) -> None:
    """直接贴 inset（带边框）"""
    H, W = base.shape[:2]
    ih, iw = inset.shape[:2]
    x = int(np.clip(x, 0, W - 1))
    y = int(np.clip(y, 0, H - 1))
    x2 = min(W, x + iw)
    y2 = min(H, y + ih)
    iw2 = x2 - x
    ih2 = y2 - y
    if iw2 <= 0 or ih2 <= 0:
        return
    base[y:y2, x:x2] = inset[:ih2, :iw2]
    if border > 0:
        cv2.rectangle(base, (x, y), (x2 - 1, y2 - 1), border_color, border)


def safe_int_box(box):
    x1, y1, x2, y2 = box
    x1i = int(math.floor(x1))
    y1i = int(math.floor(y1))
    x2i = int(math.ceil(x2))
    y2i = int(math.ceil(y2))
    return x1i, y1i, x2i, y2i


# =========================
# DS fusion with dynamic match-quality
# =========================
def get_sensor_weights(cls_name: str) -> Tuple[float, float]:
    c = canonical_class(cls_name)
    ap = PCDET_AP07.get(c, 70.0) / 100.0
    w_lidar = float(np.clip(ap, 0.15, 0.95))
    w_img = float(np.clip(1.0 - w_lidar, 0.05, 0.85))
    s = w_img + w_lidar
    return w_img / s, w_lidar / s


def ds_fuse_singleton(cls_a: str, s_a: float, w_a: float,
                      cls_b: str, s_b: float, w_b: float) -> Tuple[str, float]:
    cls_a = canonical_class(cls_a)
    cls_b = canonical_class(cls_b)

    m_a = float(np.clip(w_a * s_a, 0.0, 0.999999))
    m_b = float(np.clip(w_b * s_b, 0.0, 0.999999))
    th_a = 1.0 - m_a
    th_b = 1.0 - m_b

    K = (m_a * m_b) if (cls_a != cls_b) else 0.0
    denom = 1.0 - K + 1e-9

    if cls_a == cls_b:
        m = (m_a * m_b + m_a * th_b + th_a * m_b) / denom
        return cls_a, float(m)
    else:
        ma = (m_a * th_b) / denom
        mb = (th_a * m_b) / denom
        return (cls_a, float(ma)) if ma >= mb else (cls_b, float(mb))


def fuse_with_quality(ydet: Det2D, pdet: Det3D, q: float) -> Tuple[str, float]:
    w_img, w_lidar = get_sensor_weights(pdet.cls_name)

    gate = (0.35 + 0.65 * q)
    s_img = float(np.clip(ydet.score * gate, 0.0, 1.0))
    s_lid = float(np.clip(pdet.score * gate, 0.0, 1.0))

    fused_cls, fused_score = ds_fuse_singleton(
        ydet.cls_name, s_img, w_img,
        pdet.cls_name, s_lid, w_lidar
    )

    fused_score = float(np.clip(
        fused_score * ((1.0 - FUSED_Q_GATING_STRENGTH) + FUSED_Q_GATING_STRENGTH * q),
        0.0, 1.0
    ))
    return fused_cls, fused_score


# =========================
# 3D geometry & projection
# =========================
def boxes3d_to_corners(boxes7: np.ndarray) -> np.ndarray:
    N = boxes7.shape[0]
    corners = np.zeros((N, 8, 3), dtype=np.float32)
    for i in range(N):
        x, y, z, dx, dy, dz, yaw = boxes7[i].tolist()
        local = np.array([
            [dx / 2, dy / 2, dz / 2],
            [dx / 2, -dy / 2, dz / 2],
            [-dx / 2, -dy / 2, dz / 2],
            [-dx / 2, dy / 2, dz / 2],
            [dx / 2, dy / 2, -dz / 2],
            [dx / 2, -dy / 2, -dz / 2],
            [-dx / 2, -dy / 2, -dz / 2],
            [-dx / 2, dy / 2, -dz / 2],
        ], dtype=np.float32)
        cy = math.cos(yaw)
        sy = math.sin(yaw)
        R = np.array([[cy, -sy, 0.0],
                      [sy, cy, 0.0],
                      [0.0, 0.0, 1.0]], dtype=np.float32)
        corners[i] = (local @ R.T) + np.array([x, y, z], dtype=np.float32)
    return corners


def project_points_lidar_to_img(pts_lidar: np.ndarray, calib: Dict, T_lidar2cam: np.ndarray,
                                img_w: int, img_h: int, use_distortion=True):
    fx, fy, cx, cy = calib["fx"], calib["fy"], calib["cx"], calib["cy"]
    K = np.array([[fx, 0, cx],
                  [0, fy, cy],
                  [0, 0, 1]], dtype=np.float64)
    dist = calib["dist"].astype(np.float64) if use_distortion else None

    pts_h = np.concatenate([pts_lidar.astype(np.float64),
                            np.ones((pts_lidar.shape[0], 1), dtype=np.float64)], axis=1)
    pts_cam = (T_lidar2cam @ pts_h.T).T[:, :3]
    valid = pts_cam[:, 2] > 1e-6

    rvec = np.zeros((3, 1), dtype=np.float64)
    tvec = np.zeros((3, 1), dtype=np.float64)
    img_pts, _ = cv2.projectPoints(pts_cam, rvec, tvec, K, dist)
    return img_pts.reshape(-1, 2).astype(np.float32), valid


def get_extrinsic_matrix(calib: Dict, pts_lidar_xyz: np.ndarray, img_w: int, img_h: int) -> np.ndarray:
    T = calib["extrinsic_matrix"].copy()
    if EXTRINSIC_MODE == "lidar2cam":
        return T
    if EXTRINSIC_MODE == "cam2lidar":
        return np.linalg.inv(T)

    if pts_lidar_xyz is None or pts_lidar_xyz.shape[0] < 100:
        return T

    pts = pts_lidar_xyz
    if pts.shape[0] > 8000:
        pts = pts[np.random.choice(pts.shape[0], 8000, replace=False)]

    def score_for(Tuse):
        img_pts, valid = project_points_lidar_to_img(pts, calib, Tuse, img_w, img_h, use_distortion=USE_DISTORTION)
        img_pts = img_pts[valid]
        if img_pts.shape[0] == 0:
            return 0.0
        inside = (img_pts[:, 0] >= 0) & (img_pts[:, 0] < img_w) & (img_pts[:, 1] >= 0) & (img_pts[:, 1] < img_h)
        return float(inside.mean())

    s1 = score_for(T)
    s2 = score_for(np.linalg.inv(T))
    chosen = T if s1 >= s2 else np.linalg.inv(T)
    print(f"[CALIB] auto | lidar2cam_inlier={s1:.3f} cam2lidar_inlier={s2:.3f} -> choose {'lidar2cam' if s1 >= s2 else 'cam2lidar'}")
    return chosen


def project_boxes3d_to_2d(boxes7: np.ndarray, calib: Dict, T_lidar2cam: np.ndarray,
                          img_w: int, img_h: int, use_distortion=True):
    if boxes7.shape[0] == 0:
        return []
    corners = boxes3d_to_corners(boxes7)
    out = []
    for i in range(corners.shape[0]):
        img_pts, valid = project_points_lidar_to_img(corners[i], calib, T_lidar2cam, img_w, img_h, use_distortion)
        if valid.sum() < 4:
            out.append(None)
            continue
        xs = img_pts[valid, 0]
        ys = img_pts[valid, 1]
        box = clip_box_xyxy([float(xs.min()), float(ys.min()), float(xs.max()), float(ys.max())], img_w, img_h)
        if (box[2] - box[0]) < 2 or (box[3] - box[1]) < 2:
            out.append(None)
        else:
            out.append(box)
    return out


# =========================
# Matching (Hungarian)
# =========================
def hungarian_match(cost: np.ndarray) -> List[Tuple[int, int]]:
    try:
        from scipy.optimize import linear_sum_assignment
        r, c = linear_sum_assignment(cost)
        return list(zip(r.tolist(), c.tolist()))
    except Exception:
        matches = []
        used_r, used_c = set(), set()
        idxs = np.dstack(np.unravel_index(np.argsort(cost.ravel()), cost.shape))[0]
        for i, j in idxs:
            if i in used_r or j in used_c:
                continue
            used_r.add(int(i))
            used_c.add(int(j))
            matches.append((int(i), int(j)))
        return matches


def associate_yolo_pcdet(yolo: List[Det2D], pcdet: List[Det3D], img_w: int, img_h: int):
    N, M = len(yolo), len(pcdet)
    if N == 0 or M == 0:
        return {}, {}
    diag = math.hypot(img_w, img_h) + 1e-9
    cost = np.ones((N, M), dtype=np.float32) * 10.0

    for i in range(N):
        a = yolo[i].xyxy
        a_c = center_of_box(a)
        for j in range(M):
            b0 = pcdet[j].proj_xyxy
            if b0 is None:
                continue
            b = clip_box_xyxy(expand_box_xyxy(b0, PROJ_BOX_EXPAND_RATIO), img_w, img_h)
            b_c = center_of_box(b)

            iou = box_iou2d(a, b)
            cd = math.hypot(a_c[0] - b_c[0], a_c[1] - b_c[1]) / diag
            inside = point_in_box(a_c, b) or point_in_box(b_c, a)
            bonus = -0.15 if inside else 0.0

            cost[i, j] = COST_ALPHA * (1.0 - iou) + (1.0 - COST_ALPHA) * cd + bonus

    pairs = hungarian_match(cost)
    y2p, p2y = {}, {}
    for i, j in pairs:
        b0 = pcdet[j].proj_xyxy
        if b0 is None:
            continue
        b = clip_box_xyxy(expand_box_xyxy(b0, PROJ_BOX_EXPAND_RATIO), img_w, img_h)

        iou = box_iou2d(yolo[i].xyxy, b)
        cd = center_dist_px(yolo[i].xyxy, b)
        y_c = center_of_box(yolo[i].xyxy)
        b_c = center_of_box(b)
        inside = point_in_box(y_c, b) or point_in_box(b_c, yolo[i].xyxy)

        if (iou >= MATCH_IOU_THR) or (cd <= MATCH_CENTER_DIST_THR_PX) or inside:
            y2p[i] = j
            p2y[j] = i
    return y2p, p2y


# =========================
# FRONT “抠出来”点云裁剪：点在 OBB 内（含扩张）
# =========================
def points_in_obb(points_xyz: np.ndarray, box7: np.ndarray,
                  expand_xy: float, expand_z: float) -> np.ndarray:
    cx, cy, cz, dx, dy, dz, yaw = box7.tolist()
    px = points_xyz[:, 0] - cx
    py = points_xyz[:, 1] - cy
    pz = points_xyz[:, 2] - cz

    c = math.cos(-yaw)
    s = math.sin(-yaw)
    lx = c * px - s * py
    ly = s * px + c * py
    lz = pz

    hx = dx / 2.0 + expand_xy
    hy = dy / 2.0 + expand_xy
    hz = dz / 2.0 + expand_z

    mask = (np.abs(lx) <= hx) & (np.abs(ly) <= hy) & (np.abs(lz) <= hz)
    return mask


def rasterize_density(rr: np.ndarray, cc: np.ndarray, H: int, W: int,
                      gamma: float = 0.55,
                      dilate_ksize: int = 3) -> np.ndarray:
    """
    把离散点 (rr, cc) 转成密度灰度图（log + gamma），再转 BGR。
    """
    rr = rr.astype(np.int64)
    cc = cc.astype(np.int64)
    idx = rr * W + cc
    counts = np.bincount(idx, minlength=H * W).reshape(H, W).astype(np.float32)

    if counts.max() > 0:
        dens = np.log1p(counts)
        dens = dens / (dens.max() + 1e-6)
        dens = np.power(dens, gamma)
        gray = (dens * 255.0).astype(np.uint8)
    else:
        gray = np.zeros((H, W), dtype=np.uint8)

    if dilate_ksize and dilate_ksize >= 2:
        k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (dilate_ksize, dilate_ksize))
        gray = cv2.dilate(gray, k, iterations=1)

    bgr = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
    return bgr


def make_front_xz_crop(points: Optional[np.ndarray], det_ref: Optional[Det3D],
                       pcdet_top: Optional[Det3D], fused_top: Optional[Det3D]) -> np.ndarray:
    """
    右侧正视图：只显示 det_ref 盒子附近（盒内+扩张）的点云
    X 横轴, Z 纵轴
    """
    canvas = np.zeros((FRONT_MIN_HPX, FRONT_MIN_WPX, 3), dtype=np.uint8)
    draw_text_box(canvas, 8, 10, "FRONT (X-Z) MAIN", (220, 220, 220), 0.7, 2)

    if det_ref is None or points is None or points.shape[0] == 0:
        draw_text_box(canvas, 8, 45, "no det_ref / no points", (200, 200, 200), 0.6, 2)
        return canvas

    pts_xyz = points[:, :3]
    mask = points_in_obb(pts_xyz, det_ref.box7, FRONT_CROP_EXPAND_XY, FRONT_CROP_EXPAND_Z)
    crop = pts_xyz[mask]
    if crop.shape[0] == 0:
        draw_text_box(canvas, 8, 45, "no points in crop", (200, 200, 200), 0.6, 2)
        return canvas

    # 视图范围：用 det_ref 的 3D bbox 的 x/z extent + expand
    x, y, z, dx, dy, dz, yaw = det_ref.box7.tolist()
    x_min = (x - dx / 2 - FRONT_CROP_EXPAND_XY)
    x_max = (x + dx / 2 + FRONT_CROP_EXPAND_XY)
    z_min = (z - dz / 2 - FRONT_CROP_EXPAND_Z)
    z_max = (z + dz / 2 + FRONT_CROP_EXPAND_Z)

    # 分辨率 -> 画布大小
    res = FRONT_RESOLUTION
    W = max(FRONT_MIN_WPX, int(math.ceil((x_max - x_min) / res)))
    H = max(FRONT_MIN_HPX, int(math.ceil((z_max - z_min) / res)))

    # 画点：密度图（更清晰）
    cc = ((crop[:, 0] - x_min) / res).astype(np.int32)
    rr = ((z_max - crop[:, 2]) / res).astype(np.int32)
    cc = np.clip(cc, 0, W - 1)
    rr = np.clip(rr, 0, H - 1)
    front = rasterize_density(rr, cc, H, W, gamma=FRONT_DENSITY_GAMMA, dilate_ksize=FRONT_DILATE_KSIZE)

    # 画一个简单网格（每 1m 一条，淡线，不影响阅读）
    step_m = 1.0
    step_px = max(1, int(round(step_m / res)))
    grid_col = (30, 30, 30)
    for xg in range(0, W, step_px):
        cv2.line(front, (xg, 0), (xg, H - 1), grid_col, 1)
    for zg in range(0, H, step_px):
        cv2.line(front, (0, zg), (W - 1, zg), grid_col, 1)

    def draw_det_xz(det: Det3D, col, thick=2, tag=""):
        corners = boxes3d_to_corners(det.box7.reshape(1, 7).astype(np.float32))[0]
        xs = corners[:, 0]
        zs = corners[:, 2]
        x1, x2 = float(xs.min()), float(xs.max())
        z1, z2 = float(zs.min()), float(zs.max())

        x1p = int((x1 - x_min) / res)
        x2p = int((x2 - x_min) / res)
        y1p = int((z_max - z2) / res)
        y2p = int((z_max - z1) / res)

        x1p = int(np.clip(x1p, 0, W - 1))
        x2p = int(np.clip(x2p, 0, W - 1))
        y1p = int(np.clip(y1p, 0, H - 1))
        y2p = int(np.clip(y2p, 0, H - 1))

        cv2.rectangle(front, (x1p, y1p), (x2p, y2p), col, thick)
        # 中心点
        cxp = int(np.clip((0.5 * (x1p + x2p)), 0, W - 1))
        cyp = int(np.clip((0.5 * (y1p + y2p)), 0, H - 1))
        cv2.circle(front, (cxp, cyp), 4, col, -1)

        title = f"{tag}{det.cls_name}:{det.score:.2f}"
        draw_text_box(front, max(0, x1p), max(0, y1p - 22), title, col, 0.65, 2)

    # 右侧主视图：默认只画 fused（更容易看清）
    if fused_top is not None:
        draw_det_xz(fused_top, COL_FUSED, thick=3, tag="FUSED ")
        if VIS_SHOW_PCDET_WHEN_FUSED and pcdet_top is not None:
            draw_det_xz(pcdet_top, COL_PCDET, thick=1, tag="PCDet ")
    else:
        if pcdet_top is not None:
            draw_det_xz(pcdet_top, COL_PCDET, thick=2, tag="PCDet ")

    draw_text_box(front, 8, 10, "FRONT (X-Z) MAIN", (220, 220, 220), 0.7, 2)
    draw_text_box(front, 8, 42, f"range X[{x_min:.1f},{x_max:.1f}]m  Z[{z_min:.1f},{z_max:.1f}]m", (200, 200, 200), 0.55, 2)
    return front


# =========================
# BEV panel
# =========================
def make_bev_image(points: Optional[np.ndarray], det_ref: Optional[Det3D],
                   pcdet_top: Optional[Det3D], fused_top: Optional[Det3D]) -> np.ndarray:
    # range
    if BEV_CROP_AROUND_DET and det_ref is not None:
        cx, cy = float(det_ref.box7[0]), float(det_ref.box7[1])
        x_min, x_max = (cx - BEV_CROP_MARGIN_X), (cx + BEV_CROP_MARGIN_X)
        y_min, y_max = (cy - BEV_CROP_MARGIN_Y), (cy + BEV_CROP_MARGIN_Y)
    else:
        x_min, x_max = BEV_X_RANGE
        y_min, y_max = BEV_Y_RANGE

    res = BEV_RESOLUTION
    W = max(BEV_MIN_WPX, int(math.ceil((y_max - y_min) / res)))
    H = max(BEV_MIN_HPX, int(math.ceil((x_max - x_min) / res)))
    bev = np.zeros((H, W, 3), dtype=np.uint8)

    if points is not None and points.shape[0] > 0:
        mask = (points[:, 0] >= x_min) & (points[:, 0] <= x_max) & (points[:, 1] >= y_min) & (points[:, 1] <= y_max)
        pts = points[mask]
        if pts.shape[0] > 0:
            rr = ((x_max - pts[:, 0]) / res).astype(np.int32)
            cc = ((pts[:, 1] - y_min) / res).astype(np.int32)
            rr = np.clip(rr, 0, H - 1)
            cc = np.clip(cc, 0, W - 1)
            bev = rasterize_density(rr, cc, H, W, gamma=BEV_DENSITY_GAMMA, dilate_ksize=BEV_DILATE_KSIZE)

    # 网格淡线
    step_m = 5.0
    step_px = max(1, int(round(step_m / res)))
    grid_col = (28, 28, 28)
    for yg in range(0, W, step_px):
        cv2.line(bev, (yg, 0), (yg, H - 1), grid_col, 1)
    for xg in range(0, H, step_px):
        cv2.line(bev, (0, xg), (W - 1, xg), grid_col, 1)

    def draw_one(det: Det3D, col_bgr, thick=2, tag=""):
        x, y, z, dx, dy, dz, yaw = det.box7.tolist()
        cy_ = math.cos(yaw)
        sy_ = math.sin(yaw)
        local = np.array([[dx / 2, dy / 2],
                          [dx / 2, -dy / 2],
                          [-dx / 2, -dy / 2],
                          [-dx / 2, dy / 2]], dtype=np.float32)
        R = np.array([[cy_, -sy_],
                      [sy_, cy_]], dtype=np.float32)
        corners_xy = (local @ R.T) + np.array([x, y], dtype=np.float32)
        rr = ((x_max - corners_xy[:, 0]) / res).astype(np.int32)
        cc = ((corners_xy[:, 1] - y_min) / res).astype(np.int32)
        poly = np.stack([cc, rr], axis=1).reshape(-1, 1, 2)
        cv2.polylines(bev, [poly], True, col_bgr, thick, cv2.LINE_AA)
        rc = int((x_max - x) / res)
        cc0 = int((y - y_min) / res)
        cv2.circle(bev, (cc0, rc), 3, col_bgr, -1)
        draw_text_box(bev, max(0, cc0 + 4), max(0, rc - 20), f"{tag}{det.cls_name}:{det.score:.2f}", col_bgr, 0.55, 2)

    if fused_top is not None:
        draw_one(fused_top, COL_FUSED, thick=2, tag="F ")
        if VIS_SHOW_PCDET_WHEN_FUSED and pcdet_top is not None:
            draw_one(pcdet_top, COL_PCDET, thick=1, tag="P ")
    else:
        if pcdet_top is not None:
            draw_one(pcdet_top, COL_PCDET, thick=2, tag="P ")

    draw_text_box(bev, 10, 10, "BEV (inset)", (220, 220, 220), 0.65, 2)
    draw_text_box(bev, 10, 40, f"X[{x_min:.1f},{x_max:.1f}] Y[{y_min:.1f},{y_max:.1f}] res={res:.2f}", (200, 200, 200), 0.5, 2)
    return bev


# =========================
# Visualize helpers
# =========================
def top1_yolo(dets: List[Det2D]) -> Optional[Det2D]:
    return max(dets, key=lambda d: d.score) if dets else None


def top1_3d(dets: List[Det3D]) -> Optional[Det3D]:
    return max(dets, key=lambda d: d.score) if dets else None


def make_zoom_inset(src_img: np.ndarray,
                    ref_box: Optional[List[float]],
                    inset_size: int = 420,
                    context_expand_ratio: float = 0.65) -> Optional[np.ndarray]:
    if ref_box is None:
        return None
    H, W = src_img.shape[:2]
    box = expand_box_xyxy(ref_box, ratio=context_expand_ratio)
    box = clip_box_xyxy(box, W, H)
    x1, y1, x2, y2 = safe_int_box(box)
    x1 = max(0, min(W - 1, x1))
    y1 = max(0, min(H - 1, y1))
    x2 = max(0, min(W, x2))
    y2 = max(0, min(H, y2))
    if x2 - x1 < 2 or y2 - y1 < 2:
        return None
    crop = src_img[y1:y2, x1:x2].copy()
    inset = cv2.resize(crop, (inset_size, inset_size), interpolation=cv2.INTER_LINEAR)
    cv2.rectangle(inset, (0, 0), (inset_size - 1, inset_size - 1), (255, 255, 255), 2)
    draw_text_box(inset, 8, 10, "ZOOM", (255, 255, 255), 0.7, 2)
    return inset


# =========================
# Fusion 2D box for visualization (make fused box not identical)
# =========================
def fuse_2d_box_for_vis(yolo_box: List[float], pcd_proj_box: List[float], q: float,
                        img_w: int, img_h: int) -> List[float]:
    """
    仅用于可视化：把 YOLO 框和 PCDet 投影框做一个加权融合，让 FUSED 框在图上可区分。
    q 越高，说明两框越一致 -> 权重更平均；q 越低 -> 更偏向 YOLO（因为你想看清楚 2D 框位置）。
    """
    wy = float(np.clip(0.80 - 0.30 * q, 0.50, 0.85))  # q=1 -> 0.50, q=0 -> 0.80
    fused = [
        wy * yolo_box[0] + (1.0 - wy) * pcd_proj_box[0],
        wy * yolo_box[1] + (1.0 - wy) * pcd_proj_box[1],
        wy * yolo_box[2] + (1.0 - wy) * pcd_proj_box[2],
        wy * yolo_box[3] + (1.0 - wy) * pcd_proj_box[3],
    ]
    return clip_box_xyxy(fused, img_w, img_h)


def refine_3d_dims_with_2d(box7: np.ndarray, yolo_box: List[float], proj_box: List[float], q: float) -> np.ndarray:
    """
    可选启发式：用 2D 框宽高比例微调 3D 的 dx/dy/dz。
    默认关闭：REFINE_3D_BOX_DIMS = False
    """
    if not REFINE_3D_BOX_DIMS:
        return box7

    x, y, z, dx, dy, dz, yaw = box7.tolist()
    wy = max(1.0, (yolo_box[2] - yolo_box[0]))
    hy = max(1.0, (yolo_box[3] - yolo_box[1]))
    wp = max(1.0, (proj_box[2] - proj_box[0]))
    hp = max(1.0, (proj_box[3] - proj_box[1]))

    rw = float(np.clip(wy / wp, 0.2, 5.0))
    rh = float(np.clip(hy / hp, 0.2, 5.0))

    scale_xy = 1.0 + (rw - 1.0) * (REFINE_STRENGTH_XY * q)
    scale_z = 1.0 + (rh - 1.0) * (REFINE_STRENGTH_Z * q)

    scale_xy = float(np.clip(scale_xy, REFINE_MIN_SCALE, REFINE_MAX_SCALE))
    scale_z = float(np.clip(scale_z, REFINE_MIN_SCALE, REFINE_MAX_SCALE))

    dx2 = float(max(0.05, dx * scale_xy))
    dy2 = float(max(0.05, dy * scale_xy))
    dz2 = float(max(0.05, dz * scale_z))

    out = np.array([x, y, z, dx2, dy2, dz2, yaw], dtype=np.float32)
    return out


# =========================
# Visualize (Front main)
# =========================
def make_debug_vis(frame_id: str, img_bgr: np.ndarray,
                   yolo_top: Optional[Det2D],
                   pcdet_top: Optional[Det3D],
                   fused_top: Optional[Det3D],
                   raw_points: Optional[np.ndarray],
                   out_path: str):
    vis = img_bgr.copy()
    H, W = vis.shape[:2]
    draw_legend(vis, 12, 12)
    draw_text_box(vis, 12, H - 40, f"frame:{frame_id}", (0, 220, 220), 0.75, 2)

    # collect boxes for overlap styling
    box_map: Dict[str, Tuple[List[float], str]] = {}
    if yolo_top is not None:
        b = clip_box_xyxy(yolo_top.xyxy, W, H)
        box_map["YOLO"] = (b, f"{yolo_top.cls_name}:{yolo_top.score:.2f}")
    if pcdet_top is not None and pcdet_top.proj_xyxy is not None:
        b = clip_box_xyxy(pcdet_top.proj_xyxy, W, H)
        box_map["PCDet"] = (b, f"{pcdet_top.cls_name}:{pcdet_top.score:.2f}")
    if fused_top is not None and fused_top.proj_xyxy is not None:
        b = clip_box_xyxy(fused_top.proj_xyxy, W, H)
        box_map["FUSED"] = (b, f"{fused_top.cls_name}:{fused_top.score:.2f}")

    # compute overlap scores
    diag = math.hypot(W, H) + 1e-9
    overlap_score = {k: 0.0 for k in box_map.keys()}
    notes = []
    keys = list(box_map.keys())
    for i in range(len(keys)):
        for j in range(i + 1, len(keys)):
            ki, kj = keys[i], keys[j]
            bi, _ = box_map[ki]
            bj, _ = box_map[kj]
            is_ov, sc, info = overlap_and_score_2d(bi, bj, diag)
            if is_ov:
                overlap_score[ki] = max(overlap_score[ki], sc)
                overlap_score[kj] = max(overlap_score[kj], sc)
                notes.append(f"{ki} vs {kj}: sc={sc:.2f} iou={info['iou']:.2f} d={info['dist']:.1f}px gap={info['gap']:.1f}px")

    # draw boxes on left image
    if "YOLO" in box_map and yolo_top is not None:
        b, _ = box_map["YOLO"]
        draw_box_smart(vis, b, COL_YOLO, overlap_score.get("YOLO", 0.0), 3)
        draw_text_box(vis, int(b[0]), max(0, int(b[1]) - 26),
                      f"YOLO {yolo_top.cls_name}:{yolo_top.score:.2f}", COL_YOLO, 0.7, 2)

    if "PCDet" in box_map and pcdet_top is not None:
        b, _ = box_map["PCDet"]
        # PCDet 用更细的线，避免盖住 fused
        draw_box_smart(vis, b, COL_PCDET, overlap_score.get("PCDet", 0.0), 2)
        draw_text_box(vis, int(b[0]), min(H - 30, int(b[3]) + 6),
                      f"PCDet {pcdet_top.cls_name}:{pcdet_top.score:.2f}", COL_PCDET, 0.65, 2)

    if "FUSED" in box_map and fused_top is not None:
        b, _ = box_map["FUSED"]
        draw_box_smart(vis, b, COL_FUSED, overlap_score.get("FUSED", 0.0), 4)
        txt = f"FUSED {fused_top.cls_name}:{fused_top.score:.2f}"
        (tw, _), _ = cv2.getTextSize(txt, cv2.FONT_HERSHEY_SIMPLEX, 0.7, 2)
        draw_text_box(vis, max(0, int(b[2]) - tw - 12), max(0, int(b[1]) - 26),
                      txt, COL_FUSED, 0.7, 2)

    # overlap notes（简要）
    if notes:
        y0 = 160
        draw_text_box(vis, 12, y0, "Overlap notes:", (255, 255, 255), 0.6, 2)
        y0 += 28
        for k, line in enumerate(notes[:5]):
            draw_text_box(vis, 12, y0 + 24 * k, line, (255, 255, 255), 0.52, 2)

    # build FRONT main (right)
    det_ref = fused_top if fused_top is not None else pcdet_top
    front = make_front_xz_crop(raw_points, det_ref, pcdet_top, fused_top)

    # build BEV inset (inside left image)
    bev = make_bev_image(raw_points, det_ref, pcdet_top, fused_top)
    bev_inset_w = int(W * VIS_BEV_INSET_WIDTH_RATIO)
    bev_inset = resize_keep_aspect(bev, target_w=bev_inset_w, interp=cv2.INTER_NEAREST)
    # place at bottom-left
    paste_inset(vis, bev_inset, x=10, y=H - bev_inset.shape[0] - 10, border=2, border_color=(220, 220, 220))

    # optional zoom inset (top-right)
    if VIS_ADD_ZOOM_INSET:
        ref = None
        if fused_top is not None and fused_top.proj_xyxy is not None:
            ref = fused_top.proj_xyxy
        elif yolo_top is not None:
            ref = yolo_top.xyxy
        elif pcdet_top is not None and pcdet_top.proj_xyxy is not None:
            ref = pcdet_top.proj_xyxy

        z_inset = make_zoom_inset(vis, ref, inset_size=VIS_ZOOM_INSET_SIZE, context_expand_ratio=VIS_ZOOM_CONTEXT_EXPAND_RATIO)
        if z_inset is not None:
            paste_inset(vis, z_inset, x=W - z_inset.shape[1] - 10, y=10, border=2, border_color=(220, 220, 220))

    # fit right front to target size (right width = W * ratio, height = H)
    front_w = int(W * VIS_FRONT_WIDTH_RATIO_TO_IMG)
    front_fit = fit_to_rect(front, front_w, H, pad_color=(0, 0, 0), interp=cv2.INTER_NEAREST)

    canvas = cv2.hconcat([vis, front_fit])

    if VIS_OUT_SCALE != 1.0:
        out_w = int(round(canvas.shape[1] * VIS_OUT_SCALE))
        out_h = int(round(canvas.shape[0] * VIS_OUT_SCALE))
        canvas = cv2.resize(canvas, (out_w, out_h), interpolation=cv2.INTER_AREA)

    cv2.imwrite(out_path, canvas)


# =========================
# 3D Eval
# =========================
def iou3d_matrix_cpu(boxes_a: np.ndarray, boxes_b: np.ndarray) -> np.ndarray:
    N, M = boxes_a.shape[0], boxes_b.shape[0]
    if N == 0 or M == 0:
        return np.zeros((N, M), dtype=np.float32)

    from pcdet.ops.iou3d_nms import iou3d_nms_utils
    ta = torch.from_numpy(boxes_a).float()
    tb = torch.from_numpy(boxes_b).float()
    bev_iou = iou3d_nms_utils.boxes_bev_iou_cpu(ta, tb).cpu().numpy().astype(np.float32)

    area_a = (boxes_a[:, 3] * boxes_a[:, 4]).reshape(N, 1)
    area_b = (boxes_b[:, 3] * boxes_b[:, 4]).reshape(1, M)
    inter_area = np.where(
        bev_iou > 0,
        bev_iou * (area_a + area_b) / (1.0 + bev_iou + 1e-9),
        0.0
    ).astype(np.float32)

    zmax_a = boxes_a[:, 2] + boxes_a[:, 5] / 2.0
    zmin_a = boxes_a[:, 2] - boxes_a[:, 5] / 2.0
    zmax_b = boxes_b[:, 2] + boxes_b[:, 5] / 2.0
    zmin_b = boxes_b[:, 2] - boxes_b[:, 5] / 2.0

    overlap_h = np.maximum(
        0.0,
        np.minimum(zmax_a.reshape(N, 1), zmax_b.reshape(1, M)) -
        np.maximum(zmin_a.reshape(N, 1), zmin_b.reshape(1, M))
    ).astype(np.float32)

    inter_vol = inter_area * overlap_h
    vol_a = (boxes_a[:, 3] * boxes_a[:, 4] * boxes_a[:, 5]).reshape(N, 1)
    vol_b = (boxes_b[:, 3] * boxes_b[:, 4] * boxes_b[:, 5]).reshape(1, M)
    union_vol = vol_a + vol_b - inter_vol + 1e-9
    return (inter_vol / union_vol).astype(np.float32)


def compute_ap_from_pr(rec: np.ndarray, prec: np.ndarray) -> float:
    if rec.size == 0:
        return 0.0
    mrec = np.concatenate(([0.0], rec, [1.0]))
    mpre = np.concatenate(([0.0], prec, [0.0]))
    for i in range(mpre.size - 1, 0, -1):
        mpre[i - 1] = max(mpre[i - 1], mpre[i])
    idx = np.where(mrec[1:] != mrec[:-1])[0]
    ap = np.sum((mrec[idx + 1] - mrec[idx]) * mpre[idx + 1])
    return float(ap)


def eval_3d_map_mar(
        all_dets: Dict[str, List[Tuple[str, np.ndarray, float]]],
        all_gts: Dict[str, Dict[str, List[np.ndarray]]],
        class_names: List[str],
        iou_thr: float
) -> Dict:
    results = {"iou_thr": iou_thr, "per_class": {}, "mAP": 0.0, "mAR": 0.0}
    aps, ars = [], []

    for cls in class_names:
        dets = all_dets.get(cls, [])
        gts_by_frame = all_gts.get(cls, {})

        npos = sum(len(v) for v in gts_by_frame.values())
        if npos == 0:
            results["per_class"][cls] = {"AP": None, "AR": None, "nGT": 0, "nDet": len(dets)}
            continue

        dets_sorted = sorted(dets, key=lambda x: x[2], reverse=True)
        tp = np.zeros((len(dets_sorted),), dtype=np.float32)
        fp = np.zeros((len(dets_sorted),), dtype=np.float32)
        matched = {fid: np.zeros((len(gts_by_frame[fid]),), dtype=bool) for fid in gts_by_frame.keys()}

        for i, (fid, box, score) in enumerate(dets_sorted):
            gt_list = gts_by_frame.get(fid, [])
            if len(gt_list) == 0:
                fp[i] = 1.0
                continue
            gt_boxes = np.stack(gt_list, axis=0).astype(np.float32)
            det_box = box.reshape(1, 7).astype(np.float32)
            ious = iou3d_matrix_cpu(det_box, gt_boxes).reshape(-1)
            jmax = int(np.argmax(ious))
            if ious[jmax] >= iou_thr and (not matched[fid][jmax]):
                tp[i] = 1.0
                matched[fid][jmax] = True
            else:
                fp[i] = 1.0

        tp_cum = np.cumsum(tp)
        fp_cum = np.cumsum(fp)
        rec = tp_cum / float(npos)
        prec = tp_cum / np.maximum(tp_cum + fp_cum, 1e-9)

        ap = compute_ap_from_pr(rec, prec)
        ar = float(rec[-1]) if rec.size > 0 else 0.0

        results["per_class"][cls] = {"AP": ap, "AR": ar, "nGT": int(npos), "nDet": int(len(dets_sorted))}
        aps.append(ap)
        ars.append(ar)

    results["mAP"] = float(np.mean(aps)) if len(aps) > 0 else 0.0
    results["mAR"] = float(np.mean(ars)) if len(ars) > 0 else 0.0
    return results


def parse_pcdet_gt_label(txt_path: str) -> List[Det3D]:
    dets = []
    if not os.path.exists(txt_path):
        return dets
    with open(txt_path, "r", encoding="utf-8") as f:
        lines = [ln.strip() for ln in f.readlines() if ln.strip()]
    for ln in lines:
        parts = ln.split()
        if len(parts) < 8:
            continue
        nums = list(map(float, parts[:7]))
        cls_name = canonical_class(parts[7])
        dets.append(Det3D(box7=np.array(nums, dtype=np.float32), cls_name=cls_name, score=1.0))
    return dets


# =========================
# PCDet load compat
# =========================
def _extract_model_state(ckpt_obj):
    if isinstance(ckpt_obj, dict):
        if "model_state" in ckpt_obj:
            return ckpt_obj["model_state"]
        if "state_dict" in ckpt_obj:
            return ckpt_obj["state_dict"]
    return ckpt_obj


def _override_pcdet_score_thresh(cfg, thr: float = 0.0):
    try:
        if hasattr(cfg.MODEL, "POST_PROCESSING") and "SCORE_THRESH" in cfg.MODEL.POST_PROCESSING:
            cfg.MODEL.POST_PROCESSING.SCORE_THRESH = float(thr)
    except Exception:
        pass
    try:
        if hasattr(cfg.MODEL, "DENSE_HEAD") and hasattr(cfg.MODEL.DENSE_HEAD, "POST_PROCESSING") and "SCORE_THRESH" in cfg.MODEL.DENSE_HEAD.POST_PROCESSING:
            cfg.MODEL.DENSE_HEAD.POST_PROCESSING.SCORE_THRESH = float(thr)
    except Exception:
        pass
    try:
        if hasattr(cfg.MODEL, "ROI_HEAD") and hasattr(cfg.MODEL.ROI_HEAD, "POST_PROCESSING") and "SCORE_THRESH" in cfg.MODEL.ROI_HEAD.POST_PROCESSING:
            cfg.MODEL.ROI_HEAD.POST_PROCESSING.SCORE_THRESH = float(thr)
    except Exception:
        pass


def _ensure_map_to_bev_proj_bn_registered(model, state_dict, device):
    proj_w_key = "map_to_bev_module.proj.weight"
    bn_w_key = "map_to_bev_module.bn.weight"
    if proj_w_key not in state_dict or bn_w_key not in state_dict:
        return
    mtb = getattr(model, "map_to_bev_module", None)
    if mtb is None:
        return
    if isinstance(getattr(mtb, "proj", None), nn.Module) and isinstance(getattr(mtb, "bn", None), nn.Module):
        return

    w = state_dict[proj_w_key]
    out_ch, in_ch, kH, kW = w.shape
    use_bias = ("map_to_bev_module.proj.bias" in state_dict)
    padding = (kH // 2, kW // 2) if (kH > 1 or kW > 1) else (0, 0)

    mtb.proj = nn.Conv2d(in_ch, out_ch, kernel_size=(kH, kW), stride=1, padding=padding, bias=use_bias).to(device)
    mtb.bn = nn.BatchNorm2d(out_ch, eps=1e-3, momentum=0.01).to(device)
    print(f"[PCDet][COMPAT] Registered map_to_bev_module.proj/bn: Conv2d({in_ch}->{out_ch}, k={kH}x{kW})")


# =========================
# Load models
# =========================
def load_yolo_model(weights_path: str):
    try:
        from ultralytics import YOLO
    except Exception:
        sys.path.insert(0, ULTRALYTICS_REPO)
        from ultralytics import YOLO
    return YOLO(weights_path)


def load_pcdet_model(cfg_path: str, ckpt_path: str, device: torch.device):
    sys.path.insert(0, OPENPCDET_REPO)
    from pcdet.config import cfg, cfg_from_yaml_file
    from pcdet.datasets import DatasetTemplate
    from pcdet.models import build_network, load_data_to_gpu
    from pcdet.utils import common_utils

    class DemoDataset(DatasetTemplate):
        def __init__(self, dataset_cfg, class_names, root_path, ext=".bin", logger=None):
            super().__init__(dataset_cfg=dataset_cfg, class_names=class_names, training=False, root_path=root_path, logger=logger)
            self.root_path = Path(root_path)
            self.ext = ext
            self.points_dir = self.root_path / "points"
            self.sample_file_list = sorted(glob.glob(str(self.points_dir / f"*{self.ext}")))

        def __len__(self):
            return len(self.sample_file_list)

        def __getitem__(self, index):
            p = Path(self.sample_file_list[index])
            points = np.fromfile(str(p), dtype=np.float32).reshape(-1, 4)
            input_dict = {"points": points, "frame_id": p.stem}
            data_dict = self.prepare_data(data_dict=input_dict)
            return data_dict

    logger = common_utils.create_logger()
    cfg_from_yaml_file(cfg_path, cfg)
    _override_pcdet_score_thresh(cfg, thr=0.0)

    dataset_root = str(Path(PCDET_POINTS_DIR).parent)
    dataset = DemoDataset(cfg.DATA_CONFIG, cfg.CLASS_NAMES, dataset_root, ext=".bin", logger=logger)

    model = build_network(model_cfg=cfg.MODEL, num_class=len(cfg.CLASS_NAMES), dataset=dataset)
    model.to(device).eval()

    ckpt_obj = torch.load(ckpt_path, map_location="cpu")
    state = _extract_model_state(ckpt_obj)
    _ensure_map_to_bev_proj_bn_registered(model, state, device)

    ret = model.load_state_dict(state, strict=False)
    print(f"[PCDet] load_state_dict done. missing={len(ret.missing_keys)} unexpected={len(ret.unexpected_keys)}")
    return cfg, dataset, model, load_data_to_gpu


# =========================
# Inference
# =========================
@torch.no_grad()
def infer_yolo(yolo_model, img_bgr: np.ndarray) -> List[Det2D]:
    img_rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB)
    results = yolo_model(img_rgb, conf=YOLO_CONF_LOW, iou=0.7, verbose=False)
    r = results[0]
    dets: List[Det2D] = []
    if r.boxes is None or len(r.boxes) == 0:
        return dets
    xyxy = r.boxes.xyxy.detach().cpu().numpy()
    conf = r.boxes.conf.detach().cpu().numpy()
    cls = r.boxes.cls.detach().cpu().numpy().astype(int)
    for b, s, c in zip(xyxy, conf, cls):
        if float(s) < YOLO_CONF_LOW:
            continue
        cls_name = CLASSES[c] if 0 <= c < len(CLASSES) else str(c)
        dets.append(Det2D(xyxy=b.tolist(), cls_name=canonical_class(cls_name), score=float(s)))
    dets = sorted(dets, key=lambda d: d.score, reverse=True)[:YOLO_KEEP_TOPK]
    return dets


@torch.no_grad()
def infer_pcdet(cfg, dataset, model, load_data_to_gpu_fn, index: int, device: torch.device):
    data_dict = dataset[index]
    frame_id = data_dict["frame_id"]

    bin_path = str(Path(PCDET_POINTS_DIR) / f"{frame_id}.bin")
    raw_points = np.fromfile(bin_path, dtype=np.float32).reshape(-1, 4) if os.path.exists(bin_path) else None

    batch = dataset.collate_batch([data_dict])
    if device.type == "cuda":
        load_data_to_gpu_fn(batch)

    pred_dicts, _ = model.forward(batch)
    pred = pred_dicts[0]

    boxes = pred["pred_boxes"].detach().cpu().numpy().astype(np.float32)
    scores = pred["pred_scores"].detach().cpu().numpy().astype(np.float32)
    labels = pred["pred_labels"].detach().cpu().numpy().astype(np.int32)

    label_base = 1
    if labels.size > 0 and labels.min() == 0:
        label_base = 0

    dets3d: List[Det3D] = []
    for b, s, lb in zip(boxes, scores, labels):
        if float(s) < PCDET_CONF_LOW:
            continue
        idx_cls = int(lb) - 1 if label_base == 1 else int(lb)
        cls_name = cfg.CLASS_NAMES[idx_cls] if 0 <= idx_cls < len(cfg.CLASS_NAMES) else str(int(lb))
        dets3d.append(Det3D(box7=b.copy(), cls_name=canonical_class(cls_name), score=float(s)))

    return frame_id, raw_points, dets3d


# =========================
# Fusion
# =========================
def fuse_frame(yolo_dets: List[Det2D], pcdet_dets: List[Det3D],
               img_w: int, img_h: int, calib: Dict, T_lidar2cam: np.ndarray) -> List[Det3D]:
    # 3D->2D projection for PCDet dets
    if len(pcdet_dets) > 0:
        boxes7 = np.stack([d.box7 for d in pcdet_dets], axis=0)
        proj = project_boxes3d_to_2d(boxes7, calib, T_lidar2cam, img_w, img_h, use_distortion=USE_DISTORTION)
        for d, p in zip(pcdet_dets, proj):
            d.proj_xyxy = p

    _, p2y = associate_yolo_pcdet(yolo_dets, pcdet_dets, img_w, img_h)

    fused: List[Det3D] = []
    for pj, pdet in enumerate(pcdet_dets):
        if pj in p2y:
            ydet = yolo_dets[p2y[pj]]
            if pdet.proj_xyxy is None:
                continue

            # match quality
            q, _ = match_quality_2d(ydet.xyxy, pdet.proj_xyxy, img_w, img_h)

            fused_cls, fused_score = fuse_with_quality(ydet, pdet, q)
            if fused_score >= FUSED_KEEP_THRESH:
                # 决策级融合：默认沿用 PCDet 的 3D box7
                box7 = pdet.box7.copy()

                # 可选：启发式微调 3D 尺寸（默认关闭）
                if REFINE_3D_BOX_DIMS:
                    box7 = refine_3d_dims_with_2d(box7, ydet.xyxy, pdet.proj_xyxy, q)

                # fused 的 2D 显示框：默认做一个 yolo/proj 的加权融合，让它“看得出来不是一条线”
                fused_proj = pdet.proj_xyxy
                if FUSE_2D_BOX_FOR_VIS:
                    fused_proj = fuse_2d_box_for_vis(ydet.xyxy, pdet.proj_xyxy, q, img_w, img_h)

                fused.append(Det3D(
                    box7=box7,
                    cls_name=canonical_class(fused_cls),
                    score=float(fused_score),
                    proj_xyxy=fused_proj
                ))
        else:
            # unmatched but confident enough
            if pdet.score >= PCDET_CONF_HIGH_UNMATCHED:
                fused.append(Det3D(
                    box7=pdet.box7.copy(),
                    cls_name=pdet.cls_name,
                    score=float(pdet.score),
                    proj_xyxy=pdet.proj_xyxy
                ))
    return fused


# =========================
# Eval helpers
# =========================
def load_models():
    device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
    print(f"[INFO] device={device}")

    print("[INFO] Loading YOLO...")
    yolo_model = load_yolo_model(YOLO_WEIGHTS)

    print("[INFO] Loading PCDet...")
    cfg, dataset, pcdet_model, load_data_to_gpu_fn = load_pcdet_model(PCDET_CFG, PCDET_CKPT, device)
    print(f"[PCDet] CLASS_NAMES = {list(cfg.CLASS_NAMES)}")
    return device, yolo_model, cfg, dataset, pcdet_model, load_data_to_gpu_fn


# =========================
# Main
# =========================
def main():
    random.seed(RANDOM_SEED)
    np.random.seed(RANDOM_SEED)
    ensure_dir(DEBUG_DIR)

    device, yolo_model, cfg, dataset, pcdet_model, load_data_to_gpu_fn = load_models()

    bin_files = sorted(glob.glob(os.path.join(PCDET_POINTS_DIR, "*.bin")))
    frame_ids_all = [Path(p).stem for p in bin_files]
    valid_ids = [fid for fid in frame_ids_all if find_image_for_frame(fid) is not None]
    if not valid_ids:
        print("[ERROR] No matching (pointcloud, image) pairs found.")
        return

    # eval subset
    if NUM_EVAL_FRAMES is None:
        eval_ids = valid_ids
    else:
        eval_ids = random.sample(valid_ids, k=min(NUM_EVAL_FRAMES, len(valid_ids)))

    # debug subset
    debug_ids = random.sample(valid_ids, k=min(NUM_DEBUG_FRAMES, len(valid_ids)))

    idx_map = {Path(p).stem: i for i, p in enumerate(dataset.sample_file_list)}

    # eval storage
    all_dets_pcd: Dict[str, List[Tuple[str, np.ndarray, float]]] = {c: [] for c in CLASSES}
    all_dets_fus: Dict[str, List[Tuple[str, np.ndarray, float]]] = {c: [] for c in CLASSES}
    all_gts: Dict[str, Dict[str, List[np.ndarray]]] = {c: {} for c in CLASSES}

    chosen_T = None

    # ---------- Pass 1: eval collect ----------
    print(f"[EVAL] collecting on {len(eval_ids)} frames ...")
    for k, fid in enumerate(eval_ids):
        if fid not in idx_map:
            continue

        if (k % PROGRESS_EVERY) == 0:
            print(f"[EVAL] {k}/{len(eval_ids)} frame={fid}")

        img_path = find_image_for_frame(fid)
        if img_path is None:
            continue
        img = cv2.imread(img_path)
        if img is None:
            continue
        h, w = img.shape[:2]

        yolo_dets = infer_yolo(yolo_model, img)
        frame_id, raw_points, pcdet_dets = infer_pcdet(cfg, dataset, pcdet_model, load_data_to_gpu_fn, idx_map[fid], device)

        if chosen_T is None:
            pts_xyz = raw_points[:, :3] if raw_points is not None else None
            chosen_T = get_extrinsic_matrix(CALIB, pts_xyz, w, h)

        fused_dets = fuse_frame(yolo_dets, pcdet_dets, w, h, CALIB, chosen_T)

        # store preds baseline
        for d in pcdet_dets:
            c = canonical_class(d.cls_name)
            if c in all_dets_pcd and d.score >= EVAL_SCORE_THR_PCD:
                all_dets_pcd[c].append((fid, d.box7.copy(), float(d.score)))

        # store preds fused
        for d in fused_dets:
            c = canonical_class(d.cls_name)
            if c in all_dets_fus and d.score >= EVAL_SCORE_THR_FUSED:
                all_dets_fus[c].append((fid, d.box7.copy(), float(d.score)))

        # store GT
        gt_path = os.path.join(PCDET_GT_LABEL_DIR, f"{fid}.txt")
        gt_dets = parse_pcdet_gt_label(gt_path)
        for gd in gt_dets:
            c = canonical_class(gd.cls_name)
            if c in all_gts:
                all_gts[c].setdefault(fid, []).append(gd.box7.copy())

    # ---------- Pass 2: debug visualization ----------
    print(f"[DEBUG] saving {len(debug_ids)} visualizations into {DEBUG_DIR}/ ...")
    for k, fid in enumerate(debug_ids):
        if fid not in idx_map:
            continue

        img_path = find_image_for_frame(fid)
        if img_path is None:
            continue
        img = cv2.imread(img_path)
        if img is None:
            continue
        h, w = img.shape[:2]

        yolo_dets = infer_yolo(yolo_model, img)
        frame_id, raw_points, pcdet_dets = infer_pcdet(cfg, dataset, pcdet_model, load_data_to_gpu_fn, idx_map[fid], device)

        if chosen_T is None:
            pts_xyz = raw_points[:, :3] if raw_points is not None else None
            chosen_T = get_extrinsic_matrix(CALIB, pts_xyz, w, h)

        fused_dets = fuse_frame(yolo_dets, pcdet_dets, w, h, CALIB, chosen_T)

        y_top = top1_yolo(yolo_dets)
        p_top = top1_3d(pcdet_dets)
        f_top = top1_3d(fused_dets)

        out_path = os.path.join(DEBUG_DIR, f"{fid}.jpg")
        make_debug_vis(fid, img, y_top, p_top, f_top, raw_points, out_path)
        if (k % 5) == 0:
            print(f"[DEBUG] {k}/{len(debug_ids)} saved {out_path}")

    # ---------- Print metrics ----------
    print("\n================= 3D Evaluation =================")
    print(f"[EvalFrames] {len(eval_ids)} | PCD_thr={EVAL_SCORE_THR_PCD} | FUSED_thr={EVAL_SCORE_THR_FUSED}")
    for thr in EVAL_IOU_THRESHOLDS:
        res_p = eval_3d_map_mar(all_dets_pcd, all_gts, CLASSES, iou_thr=thr)
        res_f = eval_3d_map_mar(all_dets_fus, all_gts, CLASSES, iou_thr=thr)

        print(f"\n[IOU={thr:.2f}] PCDet baseline: mAP={res_p['mAP']:.4f}, mAR={res_p['mAR']:.4f}")
        for c in CLASSES:
            pc = res_p["per_class"].get(c, {})
            if pc.get("AP") is None:
                print(f"  - {c:14s} AP=None AR=None nGT=0 nDet={pc.get('nDet', 0)}")
            else:
                print(f"  - {c:14s} AP={pc['AP']:.4f} AR={pc['AR']:.4f} nGT={pc['nGT']} nDet={pc['nDet']}")

        print(f"[IOU={thr:.2f}] FUSED: mAP={res_f['mAP']:.4f}, mAR={res_f['mAR']:.4f}")
        for c in CLASSES:
            fc = res_f["per_class"].get(c, {})
            if fc.get("AP") is None:
                print(f"  - {c:14s} AP=None AR=None nGT=0 nDet={fc.get('nDet', 0)}")
            else:
                print(f"  - {c:14s} AP={fc['AP']:.4f} AR={fc['AR']:.4f} nGT={fc['nGT']} nDet={fc['nDet']}")

    print(f"\n[INFO] Done. Debug images saved in: {DEBUG_DIR}/")


if __name__ == "__main__":
    main()