多传感器标定基础
标定是整个 BEV 感知的地基——外参差 2 厘米、1 度， 投影就会明显错位，而模型完全学不出来。本节用 CARLA 的完美真值把原理吃透， 再故意加噪声看误差如何放大。
▸ 为什么单独用一节讲标定
纯算法教程几乎都跳过这块——因为公开数据集已经标好了，直接读参数就行。 但在车企，标定是实打实的岗位职责：新车型要标、传感器换了要标、 用久了漂移了还要重标。
更现实的是：面试聊 BEV 感知，追问两句必然到标定—— "外参不准会怎样"、"怎么发现标定漂了"。答不上来，前面讲得再好也会露怯。
本节目标
- 搞清内参和外参各自描述什么、分别怎么获得
- 掌握 CARLA 相机内参的解析计算（从 FOV 推 K 矩阵）
- 踩过并理解 CARLA/UE4 坐标系与标准相机坐标系的差异——这是本节最大的坑
- 写出激光雷达点云投影到图像的验证工具（标定对不对，一眼就看出来）
- 通过注入标定误差实验，建立"差多少会崩"的量化直觉
第一步：内参与外参到底是什么
▸ 一句话记住区别
外参会变，内参基本不变。所以量产车关注的是外参在线标定—— 车开着开着传感器松动了、被撞过了，系统要能自己发现并修正。
这也是面试常问的："你怎么知道标定漂了？" 答案就在本节的投影验证工具里——把激光雷达点投到图像，如果点云和物体边缘对不上，就是外参出问题了。
第二步：CARLA 相机内参的解析计算
真实相机的内参要靠棋盘格标定算。但 CARLA 是仿真器——它的内参可以直接算出来， 因为我们知道图像尺寸和视场角。
推导很简单：相机水平视场角 fov，图像宽 W，
则焦距（像素单位）为：
text
W / 2
f = ─────────────────────
tan(fov / 2)
CARLA 的像素是方的，所以 fx = fy = f
主点在图像中心：cx = W/2, cy = H/2
┌ f 0 W/2 ┐
K = │ 0 f H/2 │
└ 0 0 1 ┘第三步：坐标系陷阱（本节最大的坑）
这一段请务必读完。CARLA 基于 Unreal Engine，用的是 左手系，而计算机视觉标准用右手系。 直接拿 CARLA 的坐标去做投影，结果一定是错的。
| 坐标系 | X 轴 | Y 轴 | Z 轴 | 手性 |
|---|---|---|---|---|
| CARLA / UE4 | 前 forward | 右 right | 上 up | 左手系 |
| 标准相机（CV） | 右 right | 下 down | 前 forward（深度） | 右手系 |
▸ 必须做的轴变换
从 CARLA 相机坐标转到标准相机坐标，需要一次轴重排：
text
标准相机 X (右) ← CARLA Y (右)
标准相机 Y (下) ← CARLA -Z (上取负 = 下)
标准相机 Z (深度) ← CARLA X (前)
写成矩阵:
┌ 0 1 0 ┐
M = │ 0 0 -1 │ P_std = M · P_carla
└ 1 0 0 ┘忘了这一步的典型现象：投影出来的点全部堆在一条线上、 或者深度全是负的、或者图像上下颠倒。如果你的投影结果诡异，先查这里。
第四步：完整标定验证工具（本节核心产出）
这个脚本在 CARLA 里同时挂载相机和激光雷达，把点云投影到图像上并按深度着色。 标定正确的话，点云会精确贴合物体轮廓。
pythoncalib_verify.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
CARLA 相机-激光雷达标定验证
把激光雷达点云投影到相机图像上，按深度着色。
标定正确 → 点云精确贴合车辆、建筑轮廓
标定错误 → 点云整体偏移或散乱
同时支持注入标定误差，观察误差如何放大（--noise-t / --noise-r）
用法:
python calib_verify.py --frames 5 --out ./calib_out
python calib_verify.py --noise-t 0.05 --out ./calib_noise_t5cm
"""
import argparse
import os
import queue
import random
import numpy as np
import cv2
import carla
IMG_W, IMG_H, FOV = 1280, 720, 90.0
# CARLA(UE4) 相机坐标 → 标准相机坐标 的轴重排矩阵
# std_X(右) = carla_Y
# std_Y(下) = -carla_Z
# std_Z(深度) = carla_X
UE4_TO_CAM = np.array([
[0, 1, 0],
[0, 0, -1],
[1, 0, 0],
], dtype=np.float64)
def build_intrinsic(width, height, fov_deg):
"""
由图像尺寸和水平 FOV 解析计算内参矩阵
f = (W/2) / tan(fov/2)
"""
f = width / (2.0 * np.tan(np.deg2rad(fov_deg) / 2.0))
K = np.array([
[f, 0.0, width / 2.0],
[0.0, f, height / 2.0],
[0.0, 0.0, 1.0],
])
return K
def transform_to_matrix(transform):
"""
carla.Transform → 4x4 齐次变换矩阵（该传感器 → 世界）
CARLA 提供了 get_matrix()，但自己实现一遍有助于理解，
也便于后面注入误差。
"""
rot = transform.rotation
loc = transform.location
# CARLA 角度制，注意 UE4 的旋转顺序
cy, sy = np.cos(np.deg2rad(rot.yaw)), np.sin(np.deg2rad(rot.yaw))
cp, sp = np.cos(np.deg2rad(rot.pitch)), np.sin(np.deg2rad(rot.pitch))
cr, sr = np.cos(np.deg2rad(rot.roll)), np.sin(np.deg2rad(rot.roll))
M = np.identity(4)
M[0, 3], M[1, 3], M[2, 3] = loc.x, loc.y, loc.z
M[0, 0] = cp * cy
M[0, 1] = cy * sp * sr - sy * cr
M[0, 2] = -(cy * sp * cr + sy * sr)
M[1, 0] = sy * cp
M[1, 1] = sy * sp * sr + cy * cr
M[1, 2] = cy * sr - sy * sp * cr
M[2, 0] = sp
M[2, 1] = -cp * sr
M[2, 2] = cp * cr
return M
def inject_noise(M, noise_t=0.0, noise_r=0.0, seed=42):
"""
向外参注入误差，用于观察标定不准的后果
noise_t : 平移误差（米）
noise_r : 旋转误差（度）
"""
if noise_t == 0.0 and noise_r == 0.0:
return M
rng = np.random.default_rng(seed)
M = M.copy()
# 平移误差：各轴加随机扰动
if noise_t > 0:
M[:3, 3] += rng.normal(0, noise_t, 3)
# 旋转误差：绕随机轴转一个小角度
if noise_r > 0:
axis = rng.normal(0, 1, 3)
axis = axis / np.linalg.norm(axis)
angle = np.deg2rad(rng.normal(0, noise_r))
# 罗德里格斯公式
Kx = np.array([[0, -axis[2], axis[1]],
[axis[2], 0, -axis[0]],
[-axis[1], axis[0], 0]])
R_noise = (np.identity(3) + np.sin(angle) * Kx
+ (1 - np.cos(angle)) * (Kx @ Kx))
M[:3, :3] = R_noise @ M[:3, :3]
return M
def project_lidar_to_image(points_lidar, M_lidar2world, M_world2cam, K):
"""
激光雷达点云 → 图像像素（本函数是整节课的核心）
完整链条:
雷达系 --M_lidar2world--> 世界系 --M_world2cam--> CARLA相机系
--UE4_TO_CAM--> 标准相机系 --K--> 像素
参数:
points_lidar : (N, 3)
返回:
pixels (M, 2), depths (M,) —— 已过滤掉相机后方与画面外的点
"""
n = points_lidar.shape[0]
# 转齐次坐标 (4, N)
pts = np.hstack([points_lidar, np.ones((n, 1))]).T
# 1) 雷达系 → 世界系
pts_world = M_lidar2world @ pts
# 2) 世界系 → CARLA 相机系
pts_cam_ue4 = M_world2cam @ pts_world
pts_cam_ue4 = pts_cam_ue4[:3, :] # (3, N)
# 3) 轴重排：CARLA(UE4) → 标准相机（这一步最容易漏）
pts_cam = UE4_TO_CAM @ pts_cam_ue4
# 4) 过滤相机后方的点（深度必须为正）
depths = pts_cam[2, :]
valid = depths > 0.5
pts_cam = pts_cam[:, valid]
depths = depths[valid]
if pts_cam.shape[1] == 0:
return np.zeros((0, 2)), np.zeros(0)
# 5) 针孔投影
pix = K @ pts_cam
pix = pix[:2, :] / pix[2, :]
pix = pix.T # (M, 2)
# 6) 过滤画面外的点
inside = ((pix[:, 0] >= 0) & (pix[:, 0] < IMG_W) &
(pix[:, 1] >= 0) & (pix[:, 1] < IMG_H))
return pix[inside], depths[inside]
def colorize_depth(depth, max_d=50.0):
"""深度 → BGR 颜色（近红远蓝，便于肉眼判断）"""
t = np.clip(depth / max_d, 0, 1)
# 用 OpenCV 的 JET colormap
v = np.uint8((1 - t) * 255)
color = cv2.applyColorMap(v.reshape(-1, 1), cv2.COLORMAP_JET)
return color.reshape(-1, 3)
class CalibRig:
def __init__(self, host="localhost", port=2000, fps=10):
self.client = carla.Client(host, port)
self.client.set_timeout(15.0)
self.world = self.client.get_world()
self.fps = fps
self.actors = []
self._orig = None
def setup(self):
self._orig = self.world.get_settings()
s = self.world.get_settings()
s.synchronous_mode = True
s.fixed_delta_seconds = 1.0 / self.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]
spawns = self.world.get_map().get_spawn_points()
random.shuffle(spawns)
for sp in spawns:
self.vehicle = self.world.try_spawn_actor(v_bp, sp)
if self.vehicle:
break
self.actors.append(self.vehicle)
# ---- 相机 ----
c_bp = bp_lib.find("sensor.camera.rgb")
c_bp.set_attribute("image_size_x", str(IMG_W))
c_bp.set_attribute("image_size_y", str(IMG_H))
c_bp.set_attribute("fov", str(FOV))
c_bp.set_attribute("sensor_tick", "0.0")
cam_tf = carla.Transform(carla.Location(x=1.5, z=2.0))
self.camera = self.world.spawn_actor(c_bp, cam_tf, attach_to=self.vehicle)
self.cam_queue = queue.Queue()
self.camera.listen(self.cam_queue.put)
self.actors.append(self.camera)
# ---- 激光雷达 ----
l_bp = bp_lib.find("sensor.lidar.ray_cast")
l_bp.set_attribute("channels", "64")
l_bp.set_attribute("range", "80")
l_bp.set_attribute("points_per_second", "600000")
l_bp.set_attribute("rotation_frequency", str(self.fps))
l_bp.set_attribute("upper_fov", "10")
l_bp.set_attribute("lower_fov", "-30")
l_bp.set_attribute("sensor_tick", "0.0")
# 故意装在和相机不同的位置，才能体现外参的作用
lidar_tf = carla.Transform(carla.Location(x=0.0, z=2.4))
self.lidar = self.world.spawn_actor(l_bp, lidar_tf, attach_to=self.vehicle)
self.lidar_queue = queue.Queue()
self.lidar.listen(self.lidar_queue.put)
self.actors.append(self.lidar)
print("[setup] 相机 + 64 线激光雷达已挂载")
def run(self, n_frames, out_dir, noise_t=0.0, noise_r=0.0):
os.makedirs(out_dir, exist_ok=True)
K = build_intrinsic(IMG_W, IMG_H, FOV)
print(f"[calib] 内参矩阵 K:\n{K}")
if noise_t > 0 or noise_r > 0:
print(f"[calib] ⚠ 注入误差: 平移 {noise_t}m, 旋转 {noise_r}°")
self.vehicle.set_autopilot(True, self.tm.get_port())
# 预热
for _ in range(10):
self.world.tick()
self.cam_queue.get(timeout=5)
self.lidar_queue.get(timeout=5)
for i in range(n_frames):
frame = self.world.tick()
# 取本帧数据（校验帧号，保证对齐）
img = self._get_frame(self.cam_queue, frame)
pcd = self._get_frame(self.lidar_queue, frame)
# ---- 解析图像 ----
arr = np.frombuffer(img.raw_data, dtype=np.uint8)
arr = arr.reshape((IMG_H, IMG_W, 4))[:, :, :3].copy() # BGRA→BGR
# ---- 解析点云 ----
pts = np.frombuffer(pcd.raw_data, dtype=np.float32)
pts = pts.reshape(-1, 4)[:, :3] # x,y,z,intensity
# ---- 取外参（可注入误差）----
M_lidar2world = transform_to_matrix(self.lidar.get_transform())
M_cam2world = transform_to_matrix(self.camera.get_transform())
M_cam2world = inject_noise(M_cam2world, noise_t, noise_r)
M_world2cam = np.linalg.inv(M_cam2world)
# ---- 投影 ----
pix, depth = project_lidar_to_image(
pts, M_lidar2world, M_world2cam, K)
# ---- 绘制 ----
if len(pix) > 0:
colors = colorize_depth(depth)
for (u, v), c in zip(pix.astype(int), colors):
cv2.circle(arr, (u, v), 1, c.tolist(), -1)
# 信息条
tag = "PERFECT CALIB" if (noise_t == 0 and noise_r == 0) \
else f"NOISE t={noise_t}m r={noise_r}deg"
cv2.rectangle(arr, (0, 0), (430, 34), (0, 0, 0), -1)
cv2.putText(arr, f"{tag} | {len(pix)} pts", (8, 23),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1)
path = os.path.join(out_dir, f"{i:04d}.jpg")
cv2.imwrite(path, arr)
print(f" 帧 {i}: 投影 {len(pix)} 个点 → {path}")
def _get_frame(self, q, frame_id):
"""从队列取指定帧的数据"""
while True:
data = q.get(timeout=5.0)
if data.frame == frame_id:
return data
if data.frame > frame_id:
raise RuntimeError("帧号超前，同步异常")
def cleanup(self):
for a in [self.camera, self.lidar]:
try: a.stop()
except Exception: pass
if self.actors:
self.client.apply_batch(
[carla.command.DestroyActor(a) for a in self.actors if a])
if self._orig:
self.world.apply_settings(self._orig)
try: self.tm.set_synchronous_mode(False)
except Exception: pass
print("[cleanup] 完成")
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--frames", type=int, default=5)
ap.add_argument("--out", default="./calib_out")
ap.add_argument("--noise-t", type=float, default=0.0,
help="平移误差（米），试试 0.05 / 0.2")
ap.add_argument("--noise-r", type=float, default=0.0,
help="旋转误差（度），试试 1 / 3")
args = ap.parse_args()
rig = CalibRig()
try:
rig.setup()
rig.run(args.frames, args.out, args.noise_t, args.noise_r)
finally:
rig.cleanup()
if __name__ == "__main__":
main()第五步：误差敏感性实验（务必亲手跑）
这是本节最有价值的部分——建立"标定差多少会崩"的量化直觉。
bash
# 确保 CARLA 服务端在跑
conda activate ad
# ① 完美标定（基准）
python calib_verify.py --frames 3 --out ./calib_perfect
# ② 平移误差 5 厘米 —— 装配公差级别
python calib_verify.py --frames 3 --noise-t 0.05 --out ./calib_t5cm
# ③ 平移误差 20 厘米 —— 明显装错
python calib_verify.py --frames 3 --noise-t 0.20 --out ./calib_t20cm
# ④ 旋转误差 1 度 —— 轻微磕碰
python calib_verify.py --frames 3 --noise-r 1.0 --out ./calib_r1deg
# ⑤ 旋转误差 3 度
python calib_verify.py --frames 3 --noise-r 3.0 --out ./calib_r3deg
# 拉回本地对比看
# scp -r l20:~/work/calib_* ./
▸ 你会观察到的规律（这就是要建立的直觉）
| 误差类型 | 现象 | 结论 |
|---|---|---|
| 平移 5cm | 近处点云轻微偏移，远处几乎看不出 | 影响随距离衰减 |
| 平移 20cm | 近处明显错位 | 仍主要影响近处 |
| 旋转 1° | 近处还行，远处明显偏 | 影响随距离放大 |
| 旋转 3° | 远处点云完全飞出物体 | 灾难性 |
核心结论：旋转误差比平移误差危险得多。
原因是几何上的——1° 的角度误差，在 50 米处会造成约 50 × tan(1°) ≈ 0.87 米 的横向偏移，
足以让一辆车"跑"到隔壁车道。
▸ 这直接解释了 BEVFormer 的一个设计
回想 BEVFormer 用的是 可变形注意力（deformable attention）——它会在投影落点附近学一个偏移量， 而不是死板地在计算出的位置采样。
现在你知道为什么了：真实车辆的标定不可能完美， 总有零点几度的误差。可变形注意力让网络自己学会补偿这种系统性偏差， 这是它比纯几何方法鲁棒的原因之一。
常见报错与排查
| 现象 | 原因 | 解决 |
|---|---|---|
| 一个点都投影不出来 | 忘了轴重排，深度全为负 | 检查 UE4_TO_CAM 是否应用；打印 pts_cam[2,:] 看符号 |
| 点云在图像上下颠倒 | 轴重排矩阵 Y 轴符号错 | 标准相机 Y 向下，对应 CARLA 的 -Z |
| 点云全挤在一条线上 | 把 CARLA 的 X（前）当成了标准相机的 X（右） | 用本节给的矩阵，不要自己拍脑袋换轴 |
| 点云整体平移一小段 | 雷达和相机外参搞混了 | 确认 M_lidar2world 用雷达的、M_cam2world 用相机的 |
queue.Empty |
雷达 rotation_frequency 与 fps 不匹配 | 设成一致：rotation_frequency = fps |
| CARLA 启动占显存太多 | GPU 已 88% 占用 | QUALITY=Low 启动；错峰使用 |
验收清单
- 能说清内参与外参各自描述什么、哪个更容易漂移
- 能手推 CARLA 内参公式
f = (W/2)/tan(fov/2) - 理解 CARLA/UE4 左手系与标准相机右手系的差异，能写出轴重排矩阵
calib_verify.py跑通，完美标定下点云精确贴合物体轮廓- 做完 5 组误差实验，亲眼看到旋转误差比平移误差更致命
- 能解释为什么 BEVFormer 要用可变形注意力（与标定误差有关）
本节小结
- 内参描述相机自身成像，外参描述传感器装在哪——外参会漂移，是在线标定的对象
- CARLA 内参可解析计算：
f = (W/2)/tan(fov/2)，无需棋盘格标定 - CARLA/UE4 是左手系，必须做轴重排才能用标准相机模型——这是本节最大的坑
- 旋转误差远比平移误差危险：1° 在 50m 处造成近 0.9m 偏移
- 点云投影验证是判断标定是否漂移的实用工具，面试问"怎么发现标定漂了"就答这个
- 可变形注意力的存在意义之一，就是让网络自己补偿标定误差