CARLA 多相机数据采集

actor.bounding_box
    .location  →  框中心相对 actor 原点的偏移（局部系）
    .extent    →  半尺寸 (x=半长, y=半宽, z=半高)

actor.get_transform()
    .location  →  actor 在世界系的位置
    .rotation  →  actor 在世界系的朝向

要得到「框中心在世界系的位置」:
    world_center = actor_transform.transform(bbox.location)

要得到「框的完整尺寸」:
    length = extent.x * 2
    width  = extent.y * 2
    height = extent.z * 2

dataset/
├── scene_0000/
│   ├── CAM_FRONT/          000000.jpg  000001.jpg ...
│   ├── CAM_FRONT_LEFT/
│   ├── CAM_FRONT_RIGHT/
│   ├── CAM_BACK/
│   ├── CAM_BACK_LEFT/
│   ├── CAM_BACK_RIGHT/
│   ├── LIDAR_TOP/          000000.bin  （float32 × N × 4）
│   ├── calib.json          内参 + 各传感器外参（整场景固定）
│   └── frames.json         逐帧：位姿、时间戳、3D 框列表
├── scene_0001/
│   └── ...
└── dataset_info.json       全局元信息：场景列表、类别表、采集配置

#!/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
"""

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}

# CARLA 蓝图前缀 → 训练类别（简化为 3 类，够用且好学）
CATEGORY_MAP = {
    "vehicle.bicycle":    "bicycle",
    "vehicle.diamondback": "bicycle",
    "vehicle.gazelle":    "bicycle",
    "vehicle.harley":     "motorcycle",
    "vehicle.kawasaki":    "motorcycle",
    "vehicle.vespa":       "motorcycle",
    "vehicle.yamaha":      "motorcycle",
    "walker":              "pedestrian",
}
DEFAULT_CATEGORY = "car"

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


def classify_actor(type_id):
    """CARLA 蓝图 id → 训练类别"""
    for prefix, cat in CATEGORY_MAP.items():
        if type_id.startswith(prefix):
            return cat
    return 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.sensors = {}
        self.queues = {}
        self._orig = None

    # ---------------- 场景初始化 ----------------
    def setup_scene(self, town=None, weather=None, n_npc=40):
        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)

    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 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.type_id),
                "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 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("--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)
            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()

# CARLA 服务端保持运行
conda activate ad

# ① 先小规模验证（1 场景 30 帧，约 2 分钟）
python collect_bev_dataset.py --scenes 1 --frames 30 --out ./dataset_test

# ② 检查产出
tree -L 2 ./dataset_test | head -20
python -c "
import json
d=json.load(open('./dataset_test/scene_0000/frames.json'))
f=d['frames'][10]
print('帧索引:', f['index'])
print('自车速度:', f['ego_speed'], 'km/h')
print('标注框数:', len(f['boxes']))
if f['boxes']:
    b=f['boxes'][0]
    print('示例框:', b['category'], '距离', b['distance'], 'm',
          '尺寸', b['size'], '点数', b['num_lidar_pts'])
"

# ③ 确认无误后正式采集（10 场景 × 200 帧，约 1 小时）
tmux new -s collect
python collect_bev_dataset.py --scenes 10 --frames 200 --out ./dataset
# Ctrl+B D 脱离

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""数据集质量检查 —— 训练前必跑"""

import argparse
import json
import os
from collections import Counter

import numpy as np


def check(root):
    info_path = os.path.join(root, "dataset_info.json")
    if not os.path.exists(info_path):
        print("✗ 缺少 dataset_info.json")
        return
    info = json.load(open(info_path))
    cams = list(info["camera_rig"].keys())

    print("="*58)
    print("  数据集质检")
    print("="*58)

    total_frames, total_boxes = 0, 0
    cat_counter = Counter()
    dist_all, size_all, pts_all = [], [], []
    problems = []

    for scene in info["scenes"]:
        sdir = os.path.join(root, scene["name"])
        fpath = os.path.join(sdir, "frames.json")
        if not os.path.exists(fpath):
            problems.append(f"{scene['name']}: 缺少 frames.json")
            continue
        data = json.load(open(fpath))
        frames = data["frames"]
        total_frames += len(frames)

        # --- 检查 1: 各相机文件数一致 ---
        counts = {}
        for cam in cams:
            d = os.path.join(sdir, cam)
            counts[cam] = len(os.listdir(d)) if os.path.isdir(d) else 0
        lidar_dir = os.path.join(sdir, "LIDAR_TOP")
        counts["LIDAR_TOP"] = (len(os.listdir(lidar_dir))
                                if os.path.isdir(lidar_dir) else 0)

        if len(set(counts.values())) != 1:
            problems.append(f"{scene['name']}: 各传感器帧数不一致 {counts}")
        if list(counts.values())[0] != len(frames):
            problems.append(
                f"{scene['name']}: 文件数 {list(counts.values())[0]} "
                f"≠ 标注帧数 {len(frames)}")

        # --- 检查 2: 标注统计 ---
        for fr in frames:
            for b in fr["boxes"]:
                total_boxes += 1
                cat_counter[b["category"]] += 1
                dist_all.append(b["distance"])
                size_all.append(b["size"])
                pts_all.append(b["num_lidar_pts"])

                # 异常值检查
                l, w, h = b["size"]
                if not (0.3 < l < 20 and 0.3 < w < 5 and 0.3 < h < 5):
                    problems.append(
                        f"{scene['name']} 帧{fr['index']}: 尺寸异常 {b['size']}")
                if abs(b["yaw"]) > 3.15:
                    problems.append(
                        f"{scene['name']} 帧{fr['index']}: yaw 超范围 {b['yaw']}")

    # --- 汇总 ---
    print(f"场景数   : {len(info['scenes'])}")
    print(f"总帧数   : {total_frames}")
    print(f"总框数   : {total_boxes}")
    if total_frames:
        print(f"平均每帧 : {total_boxes/total_frames:.1f} 个框")

    print("\n类别分布:")
    for c, n in cat_counter.most_common():
        pct = 100 * n / max(total_boxes, 1)
        print(f"  {c:14} {n:7d}  ({pct:5.1f}%)")

    if dist_all:
        d = np.array(dist_all)
        print(f"\n距离分布: 均值 {d.mean():.1f}m, "
              f"中位数 {np.median(d):.1f}m, 最大 {d.max():.1f}m")
        for lo, hi in [(0, 10), (10, 20), (20, 30), (30, 52)]:
            n = np.sum((d >= lo) & (d < hi))
            print(f"  {lo:2d}-{hi:2d}m : {n:6d} ({100*n/len(d):5.1f}%)")

    if pts_all:
        p = np.array(pts_all)
        print(f"\n每框激光点数: 中位数 {np.median(p):.0f}, "
              f"最少 {p.min()}, 最多 {p.max()}")

    # --- 问题汇报 ---
    print("\n" + "="*58)
    if problems:
        print(f"发现 {len(problems)} 个问题（显示前 10 条）:")
        for p in problems[:10]:
            print(f"  ⚠ {p}")
    else:
        print("✓ 未发现结构性问题")

    # --- 数据健康度建议 ---
    print("\n健康度评估:")
    if total_frames < 500:
        print("  ⚠ 帧数偏少，建议至少 1000 帧再训练")
    else:
        print("  ✓ 帧数足够启动训练")

    if total_frames and total_boxes / total_frames < 3:
        print("  ⚠ 每帧目标太少，建议增加 --npc")
    else:
        print("  ✓ 目标密度合理")

    if len(cat_counter) < 2:
        print("  ⚠ 类别单一，建议加入行人（--npc 时生成 walker）")


if __name__ == "__main__":
    ap = argparse.ArgumentParser()
    ap.add_argument("--root", default="./dataset")
    check(ap.parse_args().root)