自动驾驶-M2 感知与 BEV-2.1CARLA 多相机数据采集

Your file has two real bugs (bicycles labelled as cars, and no pedestrians ever spawned). I'll fix them and write a small visual-check script so the output is something you can actually see.

不需要手敲全部，而且全敲反而是浪费。这个脚本大约 450 行，真正需要你吃透、亲手写的只有 60 行左右，其余都是 CARLA 的固定写法，复制就行。另外，你觉得"没感觉"有一部分不是你的问题：这个脚本跑完只会生成一堆 jpg、bin 和 json，什么都看不见。所以我补了一个可视化自检脚本，还修了我原代码里的两个 bug。

一、我原代码的两个 bug

1. 自行车被标成了汽车。 原代码按蓝图名前缀判断类别，但 CARLA 里根本没有 vehicle.bicycle 这个名字，vehicle.bh.crossbike 这辆自行车会被当成 car。CARLA 从 0.9.14 起，每个车辆蓝图都带一个 base_type 属性（car / truck / van / bus / motorcycle / bicycle），改成直接读它。
2. 行人永远是 0 个。 原代码只生成了车辆，没生成行人，pedestrian 这一类一个框都不会有。新版加了 --walkers 参数（默认 30 个），写法和 CARLA 自带的 generate_traffic.py 一致。

两处都修在 collect_bev_dataset.py 里了，语法检查通过。我这里没有 CARLA，没能实际跑采集，你跑的时候如果报错，把报错贴给我。

check_frame.py 是新写的自检脚本，跑完采集后执行：

python check_frame.py ./dataset/scene_0000 --frame 30

它会出两张图：一张鸟瞰图（激光点加 3D 框），一张前视图（把 3D 框投影到前视相机画面上），终端里还会打印各类别的框数。我用假数据验证过投影方向，示例见 combo.png。检查标准有三条：

• 鸟瞰图里每个框中间都有一团激光点；
• 前视图里框正好套住画面里的车和人；
• pedestrian、bicycle 两类的框数都不是 0。

这三条对上了，2.1 就算真正完成。

二、哪些手敲，哪些复制

手敲的方法：先让 AI 逐行解释，再关掉原文件，在空白文件里重写这几段，最后跑 check_frame.py 验证。能写出来、并且图对得上，才算真会了。

三、代码解释

整体流程：

连接 CARLA → 开同步模式 → 生成主车 + 6 相机 + 1 激光雷达 + NPC 车流
→ 预热 20 帧（丢弃）
→ 每一帧：tick 推进 0.1 秒 → 取齐 7 个传感器同一帧的数据
          → 存图片和点云 → 计算 3D 框真值 → 记录自车位姿
→ 写 frames.json、calib.json

1. 同步模式（第 104–111、321–326 行）

默认情况下 CARLA 一直在跑，相机和雷达各拍各的，同一帧里的图片和点云可能差几十毫秒，框就会对不上。同步模式下世界只在你调用 world.tick() 时才前进 0.1 秒。每个传感器把数据丢进自己的队列，_take 按帧号把同一时刻的 7 份数据凑齐。帧号对不上就是同步出了问题，所以要报错而不是将就用。

2. 坐标系（第 34–41 行）

CARLA 的约定是 x 向前、y 向右、z 向上，yaw 为负表示朝左。所以左前相机写成 y=-0.5, yaw=-55。nuScenes 的约定是 y 向左，正好相反，以后接开源模型时这是第一个坑。

3. 内参（第 73–78 行）

f = 图宽 / (2·tan(视场角/2))，主点取图像中心。800 像素宽、70° 视场角时，f ≈ 571。这个公式在 1.4 节推导过。

4. 3D 框真值（第 183–246 行），本节核心，分三步：

• 局部到世界：actor_tf.transform(bbox.location)。框的中心不一定在车的原点上，要先加上偏移。
• 世界到自车：乘以自车位姿的逆矩阵 get_inverse_matrix()。BEV 模型只认"相对我在哪"，不认地图上的绝对坐标。
• 朝向：目标的 yaw 减去自车的 yaw，再归一化到 ±π。
• 一个易错点：CARLA 的 extent 是半尺寸，长宽高都要乘 2。

5. 两条过滤规则（第 218、230 行）

超出 51.2 米的不要，因为超出了 BEV 范围。框里激光点少于 5 个的也不要，这等于说"传感器看不见的目标不标"。否则模型会被要求去预测它根本看不到的东西，这和 0.11 里 Occ3D 只评可见体素是同一个道理。

6. 点云（第 333–339 行）

每个点是 4 个 float32：x、y、z、强度。雷达装在自车正上方 2.5 米、没有旋转，所以从雷达系到自车系只需要 z 加 2.5。

输出目录结构

四、后面课程的学习方法

每节课都按四步走：跑通 → 看见 → 改一处 → 记一笔。

1. 跑通：先原样跑起来，别一上来就逐行啃。
2. 看见：每个结果都要可视化。2.1 用 check_frame.py，后面 2.7 节有专门的可视化工具，提前用起来。
3. 改一处：只改一个参数，先猜结果再验证。比如把 n_pts < 5 改成 < 20，猜猜远处的框会少多少。
4. 记一笔：写三行笔记：现象、原因、数字。这三行以后就是简历上的条目。

不用每节都精做。 以比亚迪的端到端和部署方向为目标，下面 10 节要精做，其余节读结论、跑一遍就行：

卡住超过两小时就停下来问。 把报错、你的猜测和已经试过的办法一起贴给我。别一个人硬扛，转行的人最容易在环境和报错上耗掉动力。

五、转行到车企的实际建议

1. 算法岗的门槛要清楚。 7.1 里引用的招聘信息显示，大厂算法岗大多要求硕士及以上。如果你的背景不完全匹配，数据闭环、仿真测试、模型部署和工具链、测试验证这几类岗位是更现实的入口。2.1 练的恰好就是这类岗位每天在做的事：数据管线、坐标系、多传感器同步。很多人是先从这些岗位进去，再转算法。
2. 车企真正筛的是工程能力：Python 和 C++、坐标变换、Linux、能自己定位 bug，再加一个能现场演示的项目。课程练的就是这些，不用追求每篇论文都读懂。
3. 边学边投，别等学完。 今天是 10 月 26 日：比亚迪秋招面试持续到 12 月，春招补录在 2–4 月，暑期实习在 3–5 月投递。学到 M3 时，简历上就已经有一条完整的"采集 → BEV → 规划"链路可以写了。

想给你更具体的路线，需要先知道你现在的背景：做什么工作、什么学历、写过多少代码？另外，要不要我把这两处修正也同步到课程 2.1 节的页面里，再把 check_frame.py 加成这一节的最后一步？

Sources:

• CARLA 0.9.14 release: vehicle blueprint base_type attribute
• CARLA 0.9.15 vehicle catalogue: bicycle/motorcycle blueprint IDs

collect_bev_dataset.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
CARLA BEV 训练数据集采集

产出: 6 路环视图像 + 激光雷达点云 + 3D 框真值 + 标定 + 自车位姿

用法:
    # 单场景快速验证
    python collect_bev_dataset.py --scenes 1 --frames 50 --out ./dataset
    # 批量多场景采集
    python collect_bev_dataset.py --scenes 10 --frames 200 --out ./dataset

v2 修正（2026-10）：
    1. 类别改用蓝图 base_type 属性判断（原版会把 vehicle.bh.crossbike 标成 car）
    2. 新增行人生成 --walkers（原版 pedestrian 类别永远为空）
"""

import argparse
import json
import math
import os
import queue
import random

import numpy as np
import carla

# ==================== 配置 ====================
IMG_W, IMG_H = 800, 450
FPS = 10

# BEV 感知范围（与训练配置保持一致，见本节说明）
PC_RANGE = 51.2

# 6 路环视布局（对齐 nuScenes 惯例）
CAMERA_RIG = {
    "CAM_FRONT":       {"x":  1.5, "y":  0.0, "z": 2.4, "yaw":    0, "fov": 70},
    "CAM_FRONT_LEFT":  {"x":  1.3, "y": -0.5, "z": 2.4, "yaw":  -55, "fov": 70},
    "CAM_FRONT_RIGHT": {"x":  1.3, "y":  0.5, "z": 2.4, "yaw":   55, "fov": 70},
    "CAM_BACK":        {"x": -1.5, "y":  0.0, "z": 2.4, "yaw":  180, "fov": 110},
    "CAM_BACK_LEFT":   {"x": -1.0, "y": -0.5, "z": 2.4, "yaw": -110, "fov": 70},
    "CAM_BACK_RIGHT":  {"x": -1.0, "y":  0.5, "z": 2.4, "yaw":  110, "fov": 70},
}

LIDAR_CFG = {"x": 0.0, "y": 0.0, "z": 2.5}

# 训练类别：car / pedestrian / bicycle / motorcycle
# [修正] 原版用蓝图名前缀判断，"vehicle.bicycle" 在 CARLA 里并不存在，
#        vehicle.bh.crossbike 会被误标成 car。0.9.14 起每个车辆蓝图都带 base_type 属性
#        （car / truck / van / bus / motorcycle / bicycle），直接用它最可靠。
BASE_TYPE_MAP = {"motorcycle": "motorcycle", "bicycle": "bicycle"}  # 其余（car/truck/van/bus）都归 car
DEFAULT_CATEGORY = "car"

# 多样性配置：不同场景用不同天气
WEATHER_PRESETS = [
    "ClearNoon", "CloudyNoon", "WetNoon",
    "HardRainNoon", "ClearSunset", "WetSunset",
]

def classify_actor(actor):
    """CARLA actor → 训练类别"""
    if actor.type_id.startswith("walker."):
        return "pedestrian"
    base = actor.attributes.get("base_type", "")
    return BASE_TYPE_MAP.get(base, DEFAULT_CATEGORY)

def build_intrinsic(width, height, fov_deg):
    """内参矩阵（公式见 1.4 节）"""
    f = width / (2.0 * math.tan(math.radians(fov_deg) / 2.0))
    return [[f, 0.0, width / 2.0],
            [0.0, f, height / 2.0],
            [0.0, 0.0, 1.0]]

class BEVDataCollector:

    def __init__(self, host="localhost", port=2000):
        self.client = carla.Client(host, port)
        self.client.set_timeout(20.0)
        self.actors = []
        self.walker_ctrls = []     # 行人 AI 控制器（清理时要先 stop 再销毁）
        self.sensors = {}
        self.queues = {}
        self._orig = None

    # ---------------- 场景初始化 ----------------
    def setup_scene(self, town=None, weather=None, n_npc=40, n_walkers=30):
        if town:
            print(f"  加载地图 {town} ...")
            self.world = self.client.load_world(town)
        else:
            self.world = self.client.get_world()

        # 天气（增加数据多样性）
        if weather:
            self.world.set_weather(getattr(carla.WeatherParameters, weather))

        # 同步模式（1.2 节讲过，采数据必须开）
        self._orig = self.world.get_settings()
        s = self.world.get_settings()
        s.synchronous_mode = True
        s.fixed_delta_seconds = 1.0 / FPS
        self.world.apply_settings(s)

        self.tm = self.client.get_trafficmanager(8000)
        self.tm.set_synchronous_mode(True)

        bp_lib = self.world.get_blueprint_library()

        # --- 主车 ---
        v_bp = bp_lib.filter("vehicle.tesla.model3")[0]
        v_bp.set_attribute("role_name", "ego")
        spawns = self.world.get_map().get_spawn_points()
        random.shuffle(spawns)
        self.ego = None
        for sp in spawns:
            self.ego = self.world.try_spawn_actor(v_bp, sp)
            if self.ego:
                break
        if self.ego is None:
            raise RuntimeError("无法生成主车")
        self.actors.append(self.ego)
        self.ego.set_autopilot(True, self.tm.get_port())

        # --- 相机 ---
        for name, cfg in CAMERA_RIG.items():
            bp = bp_lib.find("sensor.camera.rgb")
            bp.set_attribute("image_size_x", str(IMG_W))
            bp.set_attribute("image_size_y", str(IMG_H))
            bp.set_attribute("fov", str(cfg["fov"]))
            bp.set_attribute("sensor_tick", "0.0")
            tf = carla.Transform(
                carla.Location(x=cfg["x"], y=cfg["y"], z=cfg["z"]),
                carla.Rotation(yaw=cfg["yaw"]))
            cam = self.world.spawn_actor(bp, tf, attach_to=self.ego)
            q = queue.Queue()
            cam.listen(q.put)
            self.sensors[name] = cam
            self.queues[name] = q
            self.actors.append(cam)

        # --- 激光雷达 ---
        l_bp = bp_lib.find("sensor.lidar.ray_cast")
        l_bp.set_attribute("channels", "32")
        l_bp.set_attribute("range", str(PC_RANGE + 10))
        l_bp.set_attribute("points_per_second", "400000")
        l_bp.set_attribute("rotation_frequency", str(FPS))
        l_bp.set_attribute("upper_fov", "10")
        l_bp.set_attribute("lower_fov", "-30")
        l_bp.set_attribute("sensor_tick", "0.0")
        l_tf = carla.Transform(carla.Location(
            x=LIDAR_CFG["x"], y=LIDAR_CFG["y"], z=LIDAR_CFG["z"]))
        self.lidar = self.world.spawn_actor(l_bp, l_tf, attach_to=self.ego)
        self.lidar_queue = queue.Queue()
        self.lidar.listen(self.lidar_queue.put)
        self.actors.append(self.lidar)

        # --- NPC 车流 ---
        self._spawn_npc(n_npc)
        # --- 行人 ---
        # [修正] 原版只生成车辆，pedestrian 类别永远是 0 个框
        self._spawn_walkers(n_walkers)

    def _spawn_npc(self, n):
        bp_lib = self.world.get_blueprint_library()
        vehicles = bp_lib.filter("vehicle.*")
        spawns = self.world.get_map().get_spawn_points()
        random.shuffle(spawns)
        cnt = 0
        for sp in spawns:
            if cnt >= n:
                break
            npc = self.world.try_spawn_actor(random.choice(vehicles), sp)
            if npc:
                npc.set_autopilot(True, self.tm.get_port())
                self.actors.append(npc)
                cnt += 1
        print(f"  生成 {cnt} 辆 NPC")

    def _spawn_walkers(self, n):
        """生成行人 + AI 控制器（写法与 CARLA 自带 generate_traffic.py 一致）"""
        bp_lib = self.world.get_blueprint_library()
        walker_bps = bp_lib.filter("walker.pedestrian.*")
        ctrl_bp = bp_lib.find("controller.ai.walker")

        batch = []
        for _ in range(n):
            loc = self.world.get_random_location_from_navigation()
            if loc is None:
                continue
            bp = random.choice(walker_bps)
            if bp.has_attribute("is_invincible"):
                bp.set_attribute("is_invincible", "false")
            batch.append(carla.command.SpawnActor(bp, carla.Transform(loc)))
        res = self.client.apply_batch_sync(batch, True)
        walker_ids = [r.actor_id for r in res if not r.error]

        # 每个行人挂一个 AI 控制器
        batch = [carla.command.SpawnActor(ctrl_bp, carla.Transform(), wid)
                 for wid in walker_ids]
        res = self.client.apply_batch_sync(batch, True)
        ctrl_ids = [r.actor_id for r in res if not r.error]

        self.world.tick()   # 同步模式下控制器要先 tick 一次才能 start
        for cid in ctrl_ids:
            c = self.world.get_actor(cid)
            c.start()
            c.go_to_location(self.world.get_random_location_from_navigation())
            c.set_max_speed(1.0 + random.random())   # 1~2 m/s
            self.walker_ctrls.append(c)
        self.actors += [self.world.get_actor(i) for i in walker_ids]
        print(f"  生成 {len(walker_ids)} 个行人")

    # ---------------- 真值提取（本节核心） ----------------
    def extract_boxes(self, lidar_points_ego):
        """
        提取当前帧所有可见目标的 3D 框（自车坐标系）

        返回格式（每个框）:
            center : [x, y, z]   自车系下的框中心
            size   : [l, w, h]   长宽高（注意是全尺寸，不是半尺寸）
            yaw    : float       朝向（弧度，自车系）
            category, num_lidar_pts, distance
        """
        ego_tf = self.ego.get_transform()
        # 世界 → 自车 的逆变换矩阵
        world2ego = np.array(ego_tf.get_inverse_matrix())

        boxes = []
        candidates = list(self.world.get_actors().filter("vehicle.*"))
        candidates += list(self.world.get_actors().filter("walker.*"))

        for actor in candidates:
            # 排除自车（易漏，见本节说明）
            if actor.id == self.ego.id:
                continue

            bbox = actor.bounding_box
            actor_tf = actor.get_transform()

            # 框中心：局部偏移 → 世界系
            center_world = actor_tf.transform(bbox.location)
            p = np.array([center_world.x, center_world.y, center_world.z, 1.0])

            # 世界系 → 自车系
            p_ego = world2ego @ p
            x, y, z = float(p_ego[0]), float(p_ego[1]), float(p_ego[2])

            # --- 过滤规则 ①: 超出 BEV 范围 ---
            if abs(x) > PC_RANGE or abs(y) > PC_RANGE:
                continue

            # 朝向：相对自车的 yaw
            yaw = math.radians(actor_tf.rotation.yaw - ego_tf.rotation.yaw)
            # 归一化到 [-pi, pi]
            yaw = math.atan2(math.sin(yaw), math.cos(yaw))

            # --- 过滤规则 ③: 激光雷达点数（间接判断遮挡）---
            n_pts = self._count_points_in_box(
                lidar_points_ego, (x, y, z),
                (bbox.extent.x, bbox.extent.y, bbox.extent.z), yaw)
            if n_pts < 5:
                continue

            boxes.append({
                "category": classify_actor(actor),
                "center": [round(x, 3), round(y, 3), round(z, 3)],
                # extent 是半尺寸，×2 得全尺寸（易错点）
                "size": [round(bbox.extent.x * 2, 3),
                         round(bbox.extent.y * 2, 3),
                         round(bbox.extent.z * 2, 3)],
                "yaw": round(yaw, 4),
                "num_lidar_pts": int(n_pts),
                "distance": round(math.sqrt(x*x + y*y), 2),
                "actor_id": int(actor.id),   # 跨帧跟踪用
            })

        return boxes

    @staticmethod
    def _count_points_in_box(pts, center, half_size, yaw):
        """
        统计落在 3D 框内的激光点数量

        做法：把点云转到框的局部坐标系，再做轴对齐判断（简单可靠）
        """
        if pts is None or len(pts) == 0:
            return 0

        cx, cy, cz = center
        hx, hy, hz = half_size

        # 粗筛：先用外接球快速排除大部分点（性能优化）
        r = math.sqrt(hx*hx + hy*hy + hz*hz) + 0.5
        d = pts[:, :3] - np.array([cx, cy, cz])
        near = np.sum(d * d, axis=1) < r * r
        if not np.any(near):
            return 0
        d = d[near]

        # 反向旋转到框的局部系
        c, s = math.cos(-yaw), math.sin(-yaw)
        lx = d[:, 0] * c - d[:, 1] * s
        ly = d[:, 0] * s + d[:, 1] * c
        lz = d[:, 2]

        inside = ((np.abs(lx) <= hx) &
                  (np.abs(ly) <= hy) &
                  (np.abs(lz) <= hz))
        return int(np.sum(inside))

    # ---------------- 标定导出 ----------------
    def export_calib(self):
        """导出内参与外参（一个场景内固定）"""
        calib = {"cameras": {}, "lidar": {}}

        for name, cfg in CAMERA_RIG.items():
            calib["cameras"][name] = {
                "intrinsic": build_intrinsic(IMG_W, IMG_H, cfg["fov"]),
                # 外参：相机相对自车的位姿
                "translation": [cfg["x"], cfg["y"], cfg["z"]],
                "rotation_yaw_deg": cfg["yaw"],
                "fov": cfg["fov"],
                "image_size": [IMG_W, IMG_H],
            }

        calib["lidar"] = {
            "translation": [LIDAR_CFG["x"], LIDAR_CFG["y"], LIDAR_CFG["z"]],
            "rotation_yaw_deg": 0.0,
        }
        return calib

    # ---------------- 采集主循环 ----------------
    def collect(self, scene_dir, n_frames, warmup=20):
        os.makedirs(scene_dir, exist_ok=True)
        for name in CAMERA_RIG:
            os.makedirs(os.path.join(scene_dir, name), exist_ok=True)
        os.makedirs(os.path.join(scene_dir, "LIDAR_TOP"), exist_ok=True)

        # 标定单独存
        with open(os.path.join(scene_dir, "calib.json"), "w") as f:
            json.dump(self.export_calib(), f, indent=2)

        # 预热（丢弃，让物理和车流稳定）
        for _ in range(warmup):
            self.world.tick()
            for q in self.queues.values():
                q.get(timeout=10)
            self.lidar_queue.get(timeout=10)

        frames_meta = []
        for i in range(n_frames):
            frame_id = self.world.tick()

            # 取本帧所有传感器数据（帧号校验，见 1.2 节）
            images = {n: self._take(q, frame_id)
                      for n, q in self.queues.items()}
            pcd = self._take(self.lidar_queue, frame_id)

            # --- 存图像 ---
            for name, img in images.items():
                img.save_to_disk(os.path.join(scene_dir, name, f"{i:06d}.jpg"))

            # --- 存点云（裸 float32，np.fromfile 可读）---
            pts = np.frombuffer(pcd.raw_data, dtype=np.float32).reshape(-1, 4)
            pts.tofile(os.path.join(scene_dir, "LIDAR_TOP", f"{i:06d}.bin"))

            # --- 提取 3D 框真值 ---
            # 注意：点云在雷达系，框在自车系，做个平移对齐
            pts_ego = pts[:, :3].copy()
            pts_ego[:, 2] += LIDAR_CFG["z"]
            boxes = self.extract_boxes(pts_ego)

            # --- 自车位姿 ---
            tf = self.ego.get_transform()
            vel = self.ego.get_velocity()

            frames_meta.append({
                "index": i,
                "carla_frame": int(frame_id),
                "timestamp": round(i / FPS, 4),
                "ego_pose": {
                    "translation": [round(tf.location.x, 3),
                                    round(tf.location.y, 3),
                                    round(tf.location.z, 3)],
                    "rotation": [round(tf.rotation.roll, 3),
                                 round(tf.rotation.pitch, 3),
                                 round(tf.rotation.yaw, 3)],
                },
                "ego_speed": round(3.6 * math.sqrt(
                    vel.x**2 + vel.y**2 + vel.z**2), 2),
                "num_lidar_points": int(pts.shape[0]),
                "boxes": boxes,
            })

            if (i + 1) % 25 == 0:
                avg = np.mean([len(f["boxes"]) for f in frames_meta])
                print(f"    {i+1}/{n_frames} 帧, 平均每帧 {avg:.1f} 个框")

        with open(os.path.join(scene_dir, "frames.json"), "w") as f:
            json.dump({"fps": FPS, "pc_range": PC_RANGE,
                       "frames": frames_meta}, f)

        total_boxes = sum(len(f["boxes"]) for f in frames_meta)
        return {"n_frames": n_frames, "n_boxes": total_boxes}

    def _take(self, q, frame_id, timeout=10.0):
        while True:
            data = q.get(timeout=timeout)
            if data.frame == frame_id:
                return data
            if data.frame > frame_id:
                raise RuntimeError("传感器帧号超前，同步异常")

    # ---------------- 清理 ----------------
    def cleanup(self):
        for c in self.walker_ctrls:
            try: c.stop()
            except Exception: pass
        if self.walker_ctrls:
            self.client.apply_batch([carla.command.DestroyActor(c) for c in self.walker_ctrls])
        self.walker_ctrls.clear()
        for s in list(self.sensors.values()) + [getattr(self, "lidar", None)]:
            if s is None:
                continue
            try: s.stop()
            except Exception: pass
        if self.actors:
            self.client.apply_batch(
                [carla.command.DestroyActor(a) for a in self.actors if a])
        self.actors.clear()
        self.sensors.clear()
        self.queues.clear()
        if self._orig:
            self.world.apply_settings(self._orig)
        try: self.tm.set_synchronous_mode(False)
        except Exception: pass

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--scenes", type=int, default=3)
    ap.add_argument("--frames", type=int, default=100)
    ap.add_argument("--npc", type=int, default=40)
    ap.add_argument("--walkers", type=int, default=30)
    ap.add_argument("--out", default="./dataset")
    ap.add_argument("--towns", nargs="+",
                    default=["Town03", "Town05", "Town10HD"])
    args = ap.parse_args()

    os.makedirs(args.out, exist_ok=True)
    scene_infos = []

    for si in range(args.scenes):
        town = args.towns[si % len(args.towns)]
        weather = WEATHER_PRESETS[si % len(WEATHER_PRESETS)]
        scene_name = f"scene_{si:04d}"
        print(f"\n[{si+1}/{args.scenes}] {scene_name} | {town} | {weather}")

        collector = BEVDataCollector()
        try:
            collector.setup_scene(town=town, weather=weather, n_npc=args.npc,
                                  n_walkers=args.walkers)
            stat = collector.collect(
                os.path.join(args.out, scene_name), args.frames)
            scene_infos.append({
                "name": scene_name, "town": town, "weather": weather,
                **stat})
            print(f"  完成: {stat['n_frames']} 帧, {stat['n_boxes']} 个框")
        except Exception as e:
            print(f"  ✗ 场景失败: {e}")
        finally:
            collector.cleanup()

    # 全局元信息
    with open(os.path.join(args.out, "dataset_info.json"), "w") as f:
        json.dump({
            "scenes": scene_infos,
            "categories": ["car", "pedestrian", "bicycle", "motorcycle"],
            "camera_rig": CAMERA_RIG,
            "lidar_cfg": LIDAR_CFG,
            "image_size": [IMG_W, IMG_H],
            "fps": FPS,
            "pc_range": PC_RANGE,
        }, f, indent=2)

    total_f = sum(s["n_frames"] for s in scene_infos)
    total_b = sum(s["n_boxes"] for s in scene_infos)
    print(f"\n采集完成: {len(scene_infos)} 个场景, "
          f"{total_f} 帧, {total_b} 个标注框")
    print(f"输出目录: {args.out}")

if __name__ == "__main__":
    main()

check_frame.py

#!/usr/bin/env python3
"""
2.1 采集结果自检：把一帧数据"画出来"

用法:
    python check_frame.py ./dataset/scene_0000 --frame 30

输出（存在 scene 目录下的 _check/ 里）:
    bev_000030.png     鸟瞰图：激光点 + 3D 框（俯视），自车在中心
    front_000030.png   前视图：把 3D 框的 8 个角点投影到 CAM_FRONT 图像上
并在终端打印本场景各类别的框数量。

看什么:
    ① 鸟瞰图里，每个框里应该有一团激光点 → 坐标系、平移、朝向都对
    ② 前视图里，框应该套住画面里的车/人 → 内参、外参、投影都对
    ③ 类别统计里 pedestrian、bicycle 不应为 0 → 采集配置没漏
"""
import argparse
import json
import math
import os
from collections import Counter

import numpy as np
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
from PIL import Image, ImageDraw

COLORS = {"car": "#38bdf8", "pedestrian": "#f87171",
          "bicycle": "#4ade80", "motorcycle": "#fbbf24"}
LIDAR_Z = 2.5   # 与采集脚本 LIDAR_CFG["z"] 一致

def box_corners(center, size, yaw):
    """3D 框 → 8 个角点（自车系，CARLA 约定：x 前、y 右、z 上）"""
    l, w, h = size
    xs = np.array([1, 1, -1, -1, 1, 1, -1, -1]) * l / 2
    ys = np.array([1, -1, -1, 1, 1, -1, -1, 1]) * w / 2
    zs = np.array([1, 1, 1, 1, -1, -1, -1, -1]) * h / 2
    c, s = math.cos(yaw), math.sin(yaw)
    x = xs * c - ys * s + center[0]
    y = xs * s + ys * c + center[1]
    z = zs + center[2]
    return np.stack([x, y, z], 1)

def ego_to_pixel(pts, cam):
    """
    自车系点 → 像素坐标。这是手撕第 4 题的 CARLA 版本：
      1) 减去相机安装位置，再绕 z 轴转 -yaw     → 相机系（仍是 x 前 y 右 z 上）
      2) 换成针孔模型的轴：X=右=y, Y=下=-z, Z=前=x
      3) u = f·X/Z + cx,  v = f·Y/Z + cy
    """
    t = np.array(cam["translation"])
    a = -math.radians(cam["rotation_yaw_deg"])
    d = pts - t
    xc = d[:, 0] * math.cos(a) - d[:, 1] * math.sin(a)
    yc = d[:, 0] * math.sin(a) + d[:, 1] * math.cos(a)
    zc = d[:, 2]
    K = np.array(cam["intrinsic"])
    X, Y, Z = yc, -zc, xc
    u = K[0, 0] * X / np.clip(Z, 1e-6, None) + K[0, 2]
    v = K[1, 1] * Y / np.clip(Z, 1e-6, None) + K[1, 2]
    return np.stack([u, v], 1), Z

def draw_bev(pts, boxes, rng, out):
    fig, ax = plt.subplots(figsize=(7, 7), facecolor="#0d1117")
    ax.set_facecolor("#0d1117")
    m = (np.abs(pts[:, 0]) < rng) & (np.abs(pts[:, 1]) < rng)
    # 横轴 = y（右为正），纵轴 = x（前为正），和你坐在车里看到的方向一致
    ax.scatter(pts[m, 1], pts[m, 0], s=0.3, c=pts[m, 2], cmap="viridis", vmin=-0.5, vmax=3)
    for b in boxes:
        cs = box_corners(b["center"], b["size"], b["yaw"])[:4]
        poly = np.vstack([cs, cs[:1]])
        col = COLORS.get(b["category"], "w")
        ax.plot(poly[:, 1], poly[:, 0], color=col, lw=1.4)
        # 车头方向线：从中心指向前沿中点
        front = (cs[0] + cs[1]) / 2
        ax.plot([b["center"][1], front[1]], [b["center"][0], front[0]], color=col, lw=1)
    ax.add_patch(plt.Rectangle((-1, -2.3), 2, 4.6, color="w"))       # 自车
    ax.set_xlim(-rng, rng); ax.set_ylim(-rng, rng); ax.set_aspect("equal")
    ax.tick_params(colors="#9aabbc")
    ax.set_xlabel("y  (m, right +)", color="#9aabbc"); ax.set_ylabel("x  (m, forward +)", color="#9aabbc")
    for k, c in COLORS.items():
        ax.plot([], [], color=c, label=k)
    ax.legend(loc="upper right", facecolor="#161e2b", labelcolor="w", fontsize=8)
    fig.savefig(out, dpi=110, bbox_inches="tight"); plt.close(fig)

def draw_front(img_path, cam, boxes, out):
    im = Image.open(img_path).convert("RGB")
    dr = ImageDraw.Draw(im)
    edges = [(0, 1), (1, 2), (2, 3), (3, 0), (4, 5), (5, 6), (6, 7), (7, 4),
             (0, 4), (1, 5), (2, 6), (3, 7)]
    n = 0
    for b in boxes:
        uv, depth = ego_to_pixel(box_corners(b["center"], b["size"], b["yaw"]), cam)
        if (depth < 0.5).any():          # 有角点在相机背后，跳过
            continue
        if (uv[:, 0].max() < 0 or uv[:, 0].min() > im.width or
                uv[:, 1].max() < 0 or uv[:, 1].min() > im.height):
            continue
        col = COLORS.get(b["category"], "#ffffff")
        for i, j in edges:
            dr.line([tuple(uv[i]), tuple(uv[j])], fill=col, width=2)
        n += 1
    im.save(out)
    return n

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("scene_dir")
    ap.add_argument("--frame", type=int, default=0)
    ap.add_argument("--cam", default="CAM_FRONT")
    args = ap.parse_args()

    meta = json.load(open(os.path.join(args.scene_dir, "frames.json")))
    calib = json.load(open(os.path.join(args.scene_dir, "calib.json")))
    fr = meta["frames"][args.frame]
    tag = f"{args.frame:06d}"

    pts = np.fromfile(os.path.join(args.scene_dir, "LIDAR_TOP", f"{tag}.bin"),
                      dtype=np.float32).reshape(-1, 4)[:, :3].copy()
    pts[:, 2] += LIDAR_Z          # 雷达系 → 自车系（与采集脚本相同的对齐）

    out_dir = os.path.join(args.scene_dir, "_check"); os.makedirs(out_dir, exist_ok=True)
    draw_bev(pts, fr["boxes"], meta.get("pc_range", 51.2), os.path.join(out_dir, f"bev_{tag}.png"))
    n_drawn = draw_front(os.path.join(args.scene_dir, args.cam, f"{tag}.jpg"),
                         calib["cameras"][args.cam], fr["boxes"],
                         os.path.join(out_dir, f"{args.cam.lower()}_{tag}.png"))

    cnt = Counter(b["category"] for f in meta["frames"] for b in f["boxes"])
    print(f"第 {args.frame} 帧：{len(fr['boxes'])} 个框，其中 {n_drawn} 个落在 {args.cam} 画面内")
    print("整个场景各类别框数：", dict(cnt))
    for k in ("pedestrian", "bicycle", "motorcycle"):
        if cnt.get(k, 0) == 0:
            print(f"  ⚠ {k} 为 0：检查是否生成了这类目标（行人需要 --walkers）")
    pts_in = [b["num_lidar_pts"] for b in fr["boxes"]]
    if pts_in:
        print(f"每个框内激光点数：最少 {min(pts_in)}，中位数 {int(np.median(pts_in))}")
    print(f"图片已保存到 {out_dir}")

if __name__ == "__main__":
    main()

为者常成，行者常至