第05章 - 自动驾驶实战

import numpy as np

def camera_from_mount(pitch_deg: float, mount_xyz=(1.4, 0.0, 1.35)):
    """车辆坐标 -> 相机坐标的 (R_cv, t_cv)，p_cam = R_cv @ p_veh + t_cv；
    车辆系 x 前 y 左 z 上，相机系 x 右 y 下 z 前，pitch_deg > 0 为镜头下俯。"""
    th = np.deg2rad(pitch_deg); c_z = np.array([np.cos(th), 0.0, -np.sin(th)])
    c_x = np.array([0.0, -1.0, 0.0])                 # 相机 x 轴：车辆右手方向
    c_y = np.cross(c_z, c_x)                         # 右手系补全，指向车下
    R_cv = np.stack([c_x, c_y, c_z], axis=0)         # 每行是一个相机轴
    t_cv = -R_cv @ np.asarray(mount_xyz, dtype=np.float64)
    return R_cv, t_cv

def vehicle_to_pixel(pts_v, K, R_cv, t_cv):
    pts_v = np.asarray(pts_v, dtype=np.float64).reshape(-1, 3)
    p_cam = pts_v @ R_cv.T + t_cv                    # (N,3)
    uv_h = (K @ p_cam.T).T                           # (N,3) 齐次
    depth = p_cam[:, 2]
    uv = uv_h[:, :2] / uv_h[:, 2:3]
    return uv, depth

if __name__ == "__main__":
    K = np.diag([900.0, 900.0, 1.0]); K[0, 2], K[1, 2] = 640.0, 360.0
    R_cv, t_cv = camera_from_mount(pitch_deg=3.0)
    uv, depth = vehicle_to_pixel([[20.0, 0.0, -1.35]], K, R_cv, t_cv)   # 地面点
    print("uv =", uv.round(2), "depth =", depth.round(2))               # 期望 u≈640

import json
from pathlib import Path

ROOT = Path("data/lane")

def build_manifest(split="train"):
    rows, skipped = [], {"missing_mask": 0, "bad_json": 0}
    for jf in sorted((ROOT / "polygons" / split).glob("*.json")):
        img = ROOT / "images" / split / (jf.stem + ".jpg")
        try:
            ann = json.loads(jf.read_text(encoding="utf-8"))
        except json.JSONDecodeError:
            skipped["bad_json"] += 1; continue
        if not img.exists():
            skipped["missing_mask"] += 1; continue
        rows.append({
            "frame_id": ann["frame_id"],
            "image": str(img),
            "ann": str(jf),
            "n_lanes": len(ann["lanes"]),
            "has_nav": bool(ann.get("nav_line")),
        })
    (ROOT / "meta" / f"manifest_{split}.json").write_text(
        json.dumps({"split": split, "items": rows, "skipped": skipped}, ensure_ascii=False, indent=2),
        encoding="utf-8")
    print(f"{split}: {len(rows)} frames, skipped={skipped}")
    return out

if __name__ == "__main__":
    build_manifest("train"); build_manifest("val")

import json, sys
from pathlib import Path
import numpy as np

def check_frame(ann: dict) -> list[str]:  # 语义校验：顺序/间距/越界
    errs, h, w = [], ann["image_size"][1], ann["image_size"][0]
    for lane in ann["lanes"]:
        pts = np.asarray(lane["points"], dtype=np.float64)
        if len(pts) < 3:
            errs.append(f"lane {lane['id']}: 点数 {len(pts)} < 3")
            continue
        if not np.all(np.diff(pts[:, 1]) > 0):
            errs.append(f"lane {lane['id']}: v 非单调递增（应按从近到远标）")
        gap = np.linalg.norm(np.diff(pts, axis=0), axis=1)
        if gap.max() > 60:
            errs.append(f"lane {lane['id']}: 最大点间距 {gap.max():.1f}px > 60")
        if not (0 <= pts[:, 0]).all() or not (pts[:, 0] <= w).all():
            errs.append(f"lane {lane['id']}: u 越界")
        if not (0 <= pts[:, 1]).all() or not (pts[:, 1] <= h).all():
            errs.append(f"lane {lane['id']}: v 越界")
    if ann.get("nav_line"):
        nav = np.asarray(ann["nav_line"], float)
        if np.abs(np.diff(nav[:, 0])).max() > 40:
            errs.append("nav_line: 相邻点 u 跳变 > 40px，检查是否串线")
    return errs

def main(directory: str) -> int:  # CLI: python validate.py <dir>
    files, bad = sorted(Path(directory).glob("*.json")), 0
    for jf in files:
        errs = check_frame(json.loads(jf.read_text(encoding="utf-8")))
        if errs:
            bad += 1
            print(f"[FAIL] {jf.name}: " + "; ".join(errs[:3]))
    print(f"checked={len(files)} bad={bad}")
    return 1 if bad else 0

if __name__ == "__main__":
    sys.exit(main(sys.argv[1] if len(sys.argv) > 1 else "data/lane/polygons/train"))

import numpy as np

def collect_candidates(dets, img_w, img_h):
    """dets: [(cls, cx, cy, w, h, conf), ...] 归一化坐标；返回左右候选点。"""
    left, right = [], []
    for cls, cx, cy, w, h, conf in dets:
        if conf < 0.25:
            continue
        u, v = cx * img_w, cy * img_h
        if not (0 <= u < img_w and img_h * 0.45 <= v < img_h):  # 只看地平线以下
            continue
        (left if cls == 0 else right if cls == 1 else []).append((u, v))
    return (dedup_by_row(np.asarray(left, float), img_w),
            dedup_by_row(np.asarray(right, float), img_w))

def dedup_by_row(pts, img_w, row_tol=4.0, u_tol=12.0):
    """同一采样行上重复检测只保留更近的一个（v 更大）。"""
    if len(pts) < 2:
        return pts
    pts = pts[np.argsort(pts[:, 1])]
    keep = [0]
    for i in range(1, len(pts)):
        prev = pts[keep[-1]]
        if abs(pts[i, 1] - prev[1]) <= row_tol and abs(pts[i, 0] - prev[0]) < u_tol:
            keep[-1] = i if pts[i, 1] > prev[1] else keep[-1]
            continue
        keep.append(i)
    return pts[keep]

def fit_poly_ransac(uv, thresh=6.0, iters=150, min_inliers=8, seed=0):
    """三次多项式 RANSAC：u = P(v)，输入像素点，返回 (coef_vn, v0, vs, mask)。"""
    pts = np.asarray(uv, float)
    if len(pts) < min_inliers:
        return None, np.zeros(len(pts), bool)
    rng = np.random.default_rng(seed)
    v, u = pts[:, 1], pts[:, 0]
    v0, vs = float(v.min()), float(max(v.max() - v.min(), 1e-6))   # v 归一化
    vn = (v - v0) / vs
    best = (None, np.inf, None)
    for _ in range(iters):
        idx = rng.choice(len(pts), size=4, replace=False)
        try:
            coef = np.polyfit(vn[idx], u[idx], deg=3)
        except np.linalg.LinAlgError:
            continue
        inl = np.abs(np.polyval(coef, vn) - u) < thresh
        if inl.sum() < min_inliers:
            continue
        coef2 = np.polyfit(vn[inl], u[inl], deg=3)
        err = np.abs(np.polyval(coef2, vn) - u)
        score = float(np.median(err[inl]))
        if score < best[1]:
            best = (coef2, score, err < thresh)
    if best[0] is None:
        return None, np.zeros(len(pts), bool)
    coef = np.polyfit(vn[best[2]], u[best[2]], deg=3)
    return (coef, v0, vs), best[2]

def poly_u(model, v):
    """在像素 v 上求 u。model 为 fit_poly_ransac 的第一个返回值。"""
    coef, v0, vs = model
    return np.polyval(coef, (np.asarray(v, float) - v0) / vs)

def build_center_line(left_model, right_model, v_lo, v_hi, n=60):
    """左右车道线在公共 v 栅格上取中点，得到中心线像素点 (N,2)。"""
    vs = np.linspace(v_lo, v_hi, n)
    ul = poly_u(left_model, vs) if left_model else None
    ur = poly_u(right_model, vs) if right_model else None
    if ul is None and ur is None:
        raise ValueError("左右模型都为空，本帧应标记为失效")
    if ul is None:      # 只有右线：往左偏移一个车道宽（像素近似）
        center = ur - np.median(np.abs(np.diff(ur)))
    elif ur is None:
        center = ul + np.median(np.abs(np.diff(ul)))
    else:
        center = (ul + ur) / 2.0
    return np.stack([center, vs], axis=1), (ul is not None and ur is not None)

def smooth_center(center, prev=None, alpha=0.6, win=5):
    """滑动平均 + 跨帧指数平滑；alpha 越大越信当前帧。"""
    if len(center) >= win:
        center = center.copy()
        center[:, 0] = np.convolve(center[:, 0], np.ones(win) / win, mode="same")
    if prev is not None and len(prev) == len(center):
        center[:, 0] = alpha * center[:, 0] + (1 - alpha) * prev[:, 0]
    return center

import cv2, json
import numpy as np

def ipm_from_mount(K, R_cv, t_cv, img_w, img_h,
                   roi_m=(-8.0, 8.0, 3.0, 40.0), px_per_m=10.0):
    """由安装参数生成 IPM 单应矩阵。

    roi_m = (y_min, y_max, x_near, x_far)，单位 m，车辆坐标；
    px_per_m 为 BEV 栅格分辨率，10 表示 0.1 m/像素。
    """
    y_min, y_max, x_near, x_far = roi_m
    # 地面四点（车辆坐标 z=-h），取 ROI 的四个角
    corners_v = np.array([
        [x_near, y_min, -1.35], [x_near, y_max, -1.35],
        [x_far,  y_max, -1.35], [x_far,  y_min, -1.35],
    ])
    p_cam = corners_v @ R_cv.T + t_cv
    uv_h = (K @ p_cam.T).T
    src = uv_h[:, :2] / uv_h[:, 2:3]                      # 图像上的四边形
    bev_w = int((y_max - y_min) * px_per_m)
    bev_h = int((x_far - x_near) * px_per_m)
    dst = np.array([[0, bev_h], [bev_w, bev_h], [bev_w, 0], [0, 0]],
                   dtype=np.float64)
    H = cv2.getPerspectiveTransform(src.astype(np.float32), dst.astype(np.float32))
    meta = {"H": H.tolist(), "src": src.tolist(), "bev_size": [bev_w, bev_h],
            "px_per_m": px_per_m, "roi_m": list(roi_m)}  # 交付给控制侧的唯一入口
    with open("configs/ipm.json", "w", encoding="utf-8") as f:
        json.dump(meta, f, ensure_ascii=False, indent=2)
    return H, (bev_w, bev_h), src

def pixel_nav_to_bev(center_uv, H, px_per_m, bev_size):
    """导航线像素点 -> BEV 像素 -> 车辆坐标 (x 前, y 左)，单位 m。"""
    bev = cv2.perspectiveTransform(np.asarray(center_uv, np.float64).reshape(-1, 1, 2), H)
    u, v = bev[:, 0, 0], bev[:, 0, 1]   # BEV 像素坐标
    # BEV 行号向下 -> x 向前；列号向右 -> y 向左
    return np.stack([(bev_size[1] - v) / px_per_m, (bev_size[0] / 2.0 - u) / px_per_m], axis=1)

if __name__ == "__main__":
    K = np.array([[900.0, 0, 640.0], [0, 900.0, 360.0], [0, 0, 1.0]])
    R_cv = np.array([[0, -1, 0], [-0.0523, 0, -0.9986], [0.9986, 0, -0.0523]])
    H, size, _ = ipm_from_mount(K, R_cv, -R_cv @ np.array([1.4, 0, 1.35]), 1280, 720)
    vs = np.linspace(600, 720, 20)            # 一条直线车道的中心线
    pts = np.stack([640 - (vs - 600) * 0.2, vs], 1)
    print("BEV:", size, pixel_nav_to_bev(pts, H, 10.0, size)[[0, -1]].round(2).tolist())

import numpy as np

def nav_errors(pred_xy, gt_xy, xs=(3, 6, 10, 15, 20, 25, 30)):
    """pred/gt: 车辆坐标点 (N,2)，x 递增。按 x 采样比较横向误差，返回逐点误差。"""
    pred_xy = pred_xy[np.argsort(pred_xy[:, 0])]
    gt_xy = gt_xy[np.argsort(gt_xy[:, 0])]
    errs = []
    for x in xs:
        if not (gt_xy[0, 0] <= x <= gt_xy[-1, 0]):
            continue
        y_gt = np.interp(x, gt_xy[:, 0], gt_xy[:, 1])
        y_pr = np.interp(x, pred_xy[:, 0], pred_xy[:, 1], left=np.nan, right=np.nan)
        if not np.isnan(y_pr):
            errs.append(abs(y_pr - y_gt))
    return np.asarray(errs)

def summarize(frames, curv_thresh=250.0):
    """frames: [{'errs': array, 'iou': float, 'curv_radius': float, 'valid': bool}]"""
    all_err = np.concatenate([f["errs"] for f in frames if len(f["errs"])]) \
        if any(len(f["errs"]) for f in frames) else np.array([np.nan])
    cur_err = [f["errs"] for f in frames
               if len(f["errs"]) and f["curv_radius"] < curv_thresh]
    cur_err = np.concatenate(cur_err) if cur_err else np.array([np.nan])
    iou = np.mean([f["iou"] for f in frames]); invalid = np.mean([0.0 if f["valid"] else 1.0 for f in frames])
    rmse = float(np.sqrt(np.nanmean(all_err ** 2)))
    return {"iou": round(float(iou), 4), "mean_offset_m": round(float(np.nanmean(all_err)), 4),
            "curve_offset_m": round(float(np.nanmean(cur_err)), 4),
            "invalid_ratio": round(float(invalid), 4), "offset_rmse_m": round(rmse, 4)}

from ultralytics import YOLO

model = YOLO("runs/lane/kpt_yolo26/weights/best.pt")
model.export(format="engine", imgsz=1280, half=True, nms=False)       # 部署形态
res = model.predict("frame.jpg", imgsz=1280, nms=False, verbose=False)[0]

# 端到端输出已是最终框：这里只做像素换算与分桶，不再跑 NMS
dets = [(int(b.cls), *b.xywhn[0].tolist(), float(b.conf)) for b in res.boxes]
left, right = collect_candidates(dets, 1280, 720)                    # 5.2.2 的接口

import torch
import torch.nn as nn
import torch.nn.functional as F

class DoubleConv(nn.Module):
    def __init__(self, cin, cout):
        super().__init__()
        self.net = nn.Sequential(
            nn.Conv2d(cin, cout, 3, padding=1, bias=False),
            nn.BatchNorm2d(cout), nn.ReLU(inplace=True),
            nn.Conv2d(cout, cout, 3, padding=1, bias=False),
            nn.BatchNorm2d(cout), nn.ReLU(inplace=True),
        )
    def forward(self, x):
        return self.net(x)

class UNet(nn.Module):
    """2 类输出：0 背景 / 1 可行驶区域，车道线用第 2 通道单独训时可改 n_cls=3。"""
    def __init__(self, n_cls=2, base=32):
        super().__init__()
        c = [base, base * 2, base * 4, base * 8, base * 16]
        self.enc1, self.enc2 = DoubleConv(3, c[0]), DoubleConv(c[0], c[1])
        self.enc3, self.enc4 = DoubleConv(c[1], c[2]), DoubleConv(c[2], c[3])
        self.bottleneck = DoubleConv(c[3], c[4])
        self.up4 = nn.ConvTranspose2d(c[4], c[3], 2, stride=2)
        self.dec4 = DoubleConv(c[4], c[3])
        self.up3 = nn.ConvTranspose2d(c[3], c[2], 2, stride=2)
        self.dec3 = DoubleConv(c[3], c[2])
        self.up2 = nn.ConvTranspose2d(c[2], c[1], 2, stride=2)
        self.dec2 = DoubleConv(c[2], c[1])
        self.up1 = nn.ConvTranspose2d(c[1], c[0], 2, stride=2)
        self.dec1 = DoubleConv(c[1], c[0])
        self.head = nn.Conv2d(c[0], n_cls, 1)

    def forward(self, x):
        e1 = self.enc1(x)
        e2 = self.enc2(F.max_pool2d(e1, 2))
        e3 = self.enc3(F.max_pool2d(e2, 2))
        e4 = self.enc4(F.max_pool2d(e3, 2))
        b = self.bottleneck(F.max_pool2d(e4, 2))
        d4 = self.dec4(torch.cat([self.up4(b), e4], 1))
        d3 = self.dec3(torch.cat([self.up3(d4), e3], 1))
        d2 = self.dec2(torch.cat([self.up2(d3), e2], 1))
        d1 = self.dec1(torch.cat([self.up1(d2), e1], 1))
        return self.head(d1)

import torch
import torch.nn as nn
import torch.nn.functional as F

class CEDiceLoss(nn.Module):
    def __init__(self, ce_w=0.5, dice_w=0.5, ignore_index=255):
        super().__init__()
        self.ce_w, self.dice_w = ce_w, dice_w
        self.ignore_index = ignore_index

    def forward(self, logits, target):
        ce = F.cross_entropy(logits, target, ignore_index=self.ignore_index)
        prob = torch.softmax(logits, dim=1)[:, 1:]          # 跳过背景类
        valid = (target != self.ignore_index).unsqueeze(1)
        onehot = F.one_hot(target.clamp(min=0), logits.shape[1])
        onehot = onehot.permute(0, 3, 1, 2)[:, 1:].float() * valid
        prob = prob * valid
        dims = (0, 2, 3)
        inter = (prob * onehot).sum(dims)
        union = prob.sum(dims) + onehot.sum(dims)
        dice = (2 * inter + 1.0) / (union + 1.0)
        return self.ce_w * ce + self.dice_w * (1 - dice.mean())

def build_optimizer(model, lr=3e-4, wd=1e-4):
    """编码器用小学习率、解码器用大学习率，小数据集上很实用。"""
    enc, dec = [], []
    for name, p in model.named_parameters():
        (enc if name.startswith(("enc", "bottleneck")) else dec).append(p)
    return torch.optim.AdamW([
        {"params": enc, "lr": lr * 0.3},
        {"params": dec, "lr": lr},
    ], weight_decay=wd)

if __name__ == "__main__":
    logits = torch.randn(2, 3, 64, 96, requires_grad=True)
    target = torch.randint(0, 3, (2, 64, 96)); target[:, :, :40] = 1   # 大半是路
    loss = CEDiceLoss()(logits, target); loss.backward()
    print("loss:", round(float(loss), 4), "grad ok:", logits.grad is not None)

import torch
from torch.amp import GradScaler, autocast

def train_one_epoch(model, loader, crit, opt, scaler, device, sched=None):
    model.train(); running, n = 0.0, 0
    for step, (img, mask) in enumerate(loader):
        img, mask = img.to(device, non_blocking=True), mask.to(device, non_blocking=True)
        opt.zero_grad(set_to_none=True)
        with autocast("cuda", dtype=torch.float16):
            loss = crit(model(img), mask)
        scaler.scale(loss).backward()
        scaler.unscale_(opt)
        torch.nn.utils.clip_grad_norm_(model.parameters(), max_norm=5.0)
        scaler.step(opt)
        scaler.update()
        if sched is not None:
            sched.step()
        running += float(loss.detach()); n += 1
        if step % 50 == 0:
            print(f"step={step} loss={running / n:.4f} lr={opt.param_groups[-1]['lr']:.2e}")
    return running / max(n, 1)

@torch.no_grad()
def evaluate(model, loader, device, n_cls=3):
    """按类统计 IoU，返回 dict；比整体像素准确率更能反映小目标好坏。"""
    model.eval()
    inter = torch.zeros(n_cls, device=device)
    union = torch.zeros(n_cls, device=device)
    for img, mask in loader:
        pred = model(img.to(device)).argmax(1)
        for c in range(n_cls):
            inter[c] += ((pred == c) & (mask.to(device) == c)).sum()
            union[c] += ((pred == c) | (mask.to(device) == c)).sum()
    return {f"iou_{c}": round(float(inter[c] / union[c].clamp(min=1)), 4) for c in range(n_cls)}

import albumentations as A

def build_augment(phase="train", h=544, w=960):
    if phase == "train":
        return A.Compose([
            A.RandomResizedCrop(size=(h, w), scale=(0.6, 1.0), ratio=(1.6, 2.2)),
            A.HorizontalFlip(p=0.0),                 # 车道线左右语义固定，不翻转
            A.RandomBrightnessContrast(brightness_limit=0.3, contrast_limit=0.3, p=0.6),
            A.HueSaturationValue(hue_shift_limit=8, sat_shift_limit=25, p=0.4),
            A.RandomFog(fog_coef_lower=0.1, fog_coef_upper=0.35, p=0.15),
            A.RandomRain(slant_lower=-10, slant_upper=10, p=0.15),
            A.RandomShadow(p=0.2),
        ])
    return A.Compose([A.Resize(h, w)])

if __name__ == "__main__":
    import numpy as np
    out = build_augment("train")(image=np.zeros((720, 1280, 3), np.uint8),
                                 mask=np.zeros((720, 1280), np.uint8))
    assert out["image"].shape[:2] == out["mask"].shape[:2]   # 掩码必须同步几何变换

import numpy as np
from skimage.morphology import skeletonize, remove_small_objects
import cv2

def drivable_to_graph_free(drivable: np.ndarray, min_area=800):
    """可行驶区域掩码 -> 骨架（bool）：开运算去噪、去碎块、骨架化。"""
    m = cv2.morphologyEx((drivable > 0).astype(np.uint8), cv2.MORPH_OPEN,
                         np.ones((5, 5), np.uint8))
    return skeletonize(remove_small_objects(m.astype(bool), min_size=min_area))

def shortest_path(skel: np.ndarray, cost_map: np.ndarray, start_xy, end_y):
    """在骨架栅格上找 start -> 最远行的最短路径，返回 [(u,v), ...]。

    代价 = 骨架外高代价（不可走）+ 与参考中心线的横向偏离代价。
    这里用简化的动态规划：逐行向下传播，只在骨架像素上转移动。
    """
    h, w = skel.shape
    rows = np.where(skel.any(axis=1))[0]
    rows = rows[rows <= end_y] if len(rows) else rows
    if len(rows) < 5:
        return []
    inf = np.float32(1e9)
    dp = np.full((len(rows), w), inf, dtype=np.float32)
    par = np.full((len(rows), w), -1, dtype=np.int32)
    su = int(np.clip(start_xy[0], 0, w - 1))
    dp[0, su] = 0.0
    for i in range(1, len(rows)):
        v = rows[i]
        prev = dp[i - 1]
        for u in np.where(skel[v])[0]:
            lo, hi = max(0, u - 6), min(w, u + 7)
            cand = prev[lo:hi] + cost_map[v, lo:hi] + 0.05 * np.abs(np.arange(lo, hi) - u)
            j = int(np.argmin(cand))
            if cand[j] < inf / 2:
                dp[i, u] = cand[j]
                par[i, u] = lo + j
    # 回溯：终点取最后一行里代价最小的骨架点
    last = int(np.argmin(dp[-1]))
    if dp[-1, last] >= inf / 2:
        return []
    path, u = [], last
    for i in range(len(rows) - 1, -1, -1):
        path.append((u, int(rows[i])))
        u = int(par[i, u])
        if u < 0:
            break
    return path[::-1]

from ultralytics import YOLO

model = YOLO("yolo26n-sem.pt")
res = model.predict("frame.jpg", imgsz=960, verbose=False)[0]
# 语义分割的输出字段随版本演进，先确认字段名再写死下游
print(sorted(k for k in vars(res) if not k.startswith("_")))

import random

@dataclass
class SceneRandomizer:
    seed: int = 0
    def __post_init__(self):
        self.rng = random.Random(self.seed)

    def per_scene(self):
        r = self.rng
        return {"sun_azimuth_deg": r.uniform(0, 360),
                "road_roughness": r.uniform(0.2, 0.9),
                "lane_wear": r.uniform(0.0, 0.4),
                "camera_height_m": 1.35 + r.uniform(-0.15, 0.15),
                "camera_pitch_deg": 3.0 + r.uniform(-2.5, 2.5),
                "focal_scale": 1.0 + r.uniform(-0.04, 0.04),
                "texture_id": r.randint(0, 11)}

    def per_frame(self):
        r = self.rng
        return {"sun_elevation_deg": r.uniform(5, 75),
                "light_intensity": r.uniform(0.3, 1.6),
                "fog": r.uniform(0.0, 0.35) if r.random() < 0.30 else 0.0,
                "rain": r.uniform(0.0, 0.8) if r.random() < 0.20 else 0.0}

import json
import numpy as np

def project_3d_box(corners_world, view_mat, proj_mat, img_w, img_h):
    """3D 框 8 角点（世界系）-> 2D 框 (x1,y1,x2,y2)；None 表示全在画面外/相机后方。
    view_mat/proj_mat 为 4x4 行主序矩阵。"""
    pw = np.concatenate([corners_world, np.ones((8, 1))], axis=1)     # (8,4)
    cam = pw @ view_mat.T                                             # 相机空间
    if np.all(cam[:, 2] <= 1e-3):
        return None
    clip = cam @ proj_mat.T
    ndc = clip[:, :3] / np.where(np.abs(clip[:, 3:4]) < 1e-9, 1e-9, clip[:, 3:4])
    u = (ndc[:, 0] * 0.5 + 0.5) * img_w
    v = (1.0 - (ndc[:, 1] * 0.5 + 0.5)) * img_h
    x1, y1, x2, y2 = float(u.min()), float(v.min()), float(u.max()), float(v.max())
    if x2 < 0 or x1 > img_w or y2 < 0 or y1 > img_h:
        return None
    return [max(0.0, x1), max(0.0, y1), min(float(img_w), x2), min(float(img_h), y2)]

import math, torch
from torch.utils.data import WeightedRandomSampler

def make_sim2real_sampler(sources, sim_weight=0.4, num_samples=20000, seed=7):
    """sources: [(n_frames, is_synthetic), ...]，合成样本权重可调。"""
    weights = [w for n, s in sources for w in [(sim_weight if s else 1.0)] * n]
    return WeightedRandomSampler(torch.tensor(weights, dtype=torch.double),
                                 num_samples=num_samples, replacement=True,
                                 generator=torch.Generator().manual_seed(seed))

def finetune_lr_schedule(epoch, base_lr=3e-4, warmup=2, total=12):
    """真实数据微调：前 warmup 轮线性升温到 1/10，之后余弦衰减。"""
    if epoch < warmup:
        return base_lr * 0.1 * (epoch + 1) / warmup
    t = (epoch - warmup) / max(1, total - warmup)
    return base_lr * 0.1 * 0.5 * (1 + math.cos(math.pi * t))

import numpy as np

def offset_camera(c2w, lateral_m, up_m):
    """在原相机位姿上叠加横向/垂向平移。c2w 为 4x4 相机到世界矩阵（OpenCV 约定）。

    相机系 x 右、y 下、z 前，所以「上」是 -y。"""
    t = np.array([lateral_m, -up_m, 0.0])
    out = c2w.copy()
    out[:3, 3] = c2w[:3, 3] + c2w[:3, :3] @ t
    return out