自动驾驶-nuscenes 3D 投影代码深度解读

下边的nuscenes 3D 投影代码，我的理解主要是复现在全局坐标中标记（人工标记）的3D，然后基于自车坐标在全局，相机和车辆，相机和像素，然后计算出来后，直接将图画在图片上，用3D 框画出来，再标注类型。我的理解是否准确，请你对这个代码进行深入解读，对流程或关键理解处画图，对于每个函数或方法解读，为什么最终在图片上画出 2D/3D 框，需要做这么多转换工作，一步一步这么做。

你的理解大方向是对的，但有三处需要校准。先把判定说清楚，再逐层拆。

一、先校准你的三个理解

1. "在全局坐标中人工标记的 3D" —— 半对。

标注员实际不是对着全局坐标标的，而是在 LiDAR 关键帧融合出来的点云里画 3D 框。但 nuScenes 在 sample_annotation 里把结果存成了全局坐标（translation / size / rotation）。

为什么要存全局？因为同一个物体要被 6 路相机 + LiDAR + 前后帧共享。如果存成"相机系坐标"，6 路相机就要存 6 份，下一帧车动了还要重存。存全局系，一个物体在一段时间内就是一条记录，谁想用谁自己变换过去。这就是为什么后面所有的"麻烦"都是必然的。

2. "直接将图画在图片上，用 3D 框画出来" —— 对，但要说清楚画的是什么。

代码从头到尾没有算过一个 2D bounding box。它算的是 3D 框的 8 个角点，把这 8 个点分别投影成 8 个像素，再连 12 条边。所以你看到的"立体框"是 3D 结构的二维投影，不是矩形框。这是个重要区别：3D 框的投影通常不是矩形，是个歪的六边形轮廓。

3. "自车坐标在全局，相机和车辆，相机和像素" —— 你漏说了一层，而且漏了最关键的性质。

链条是四个坐标系、三步变换，其中前两步是刚体变换的逆，第三步是投影（不可逆、降维）。

二、四个坐标系与三步变换

每一步都在消掉一类"跟物体本身无关的变量"：第一步消掉自车走到哪、朝哪；第二步消掉相机装在车的哪个位置、朝哪个角度；第三步消掉深度。剩下的才是这张照片上该出现的东西。

三、为什么前两步必须"取逆"——这是最容易翻车的地方

ego_pose 和 calibrated_sensor 这两条记录，描述的方向和你想要的方向是相反的。

数学上，ego_pose 给的是"车身系的点变到全局系"：

p_global = R · p_ego + t

反解出来就是：

p_ego = Rᵀ · (p_global − t)

注意括号的位置。这是整段代码最容易写错的一行——Rᵀ·p − t 和 Rᵀ·(p − t) 结果完全不同，前者会让所有框整体飘走，而且飘的方向随车的朝向变化，调试时特别迷惑。代码里写的是：

points = points - np.array(ego_pose["translation"]).reshape(3, 1)   # 先减 t
points = np.dot(Quaternion(...).rotation_matrix.T, points)          # 再乘 Rᵀ

先减后乘，顺序是对的。

另外 .T 之所以能当作求逆用，是因为旋转矩阵是正交阵，R⁻¹ = Rᵀ。这里省了一次矩阵求逆，也避免了数值误差。相机外参 calib 那一步同理，calib 描述的是"相机在车身系的位姿"，方向同样是反的。

四、逐函数解读

box_corners(center, size, rotation) —— 把 3 个数字变成 8 个点

标注文件里一个框只有 9 个数：中心 3 个、尺寸 3 个、四元数 4 个（共 10 个数）。要画线就必须先展开成 8 个角点。

函数分两步走。先在物体自身坐标系（原点在框中心，x 沿车长、y 沿车宽、z 朝上）用尺寸的一半生成 8 个角点，这时候框是正着摆的；然后 q.rotation_matrix @ corners 把它转到正确朝向，再 + center 平移到全局位置。

有个坑：nuScenes 的 size 顺序是 [width, length, height]，也就是宽在前长在后。代码里 w, l, h = size 是对的，但如果你按直觉写成 l, w, h，车会变成横着的，而且只有在长宽差别大的车（比如卡车）上才明显，小轿车上几乎看不出来——非常隐蔽的 bug。

global_to_camera(points, ego_pose, calib) —— 两次刚体逆变换

这就是上面推的公式，连做两遍。值得注意的是它一次处理 (3, N) 的整批点，靠 numpy 广播完成平移，比逐点循环快得多。

camera_to_pixel(points, K) —— 针孔投影

[u']   [fx  0  cx] [X]
[v'] = [ 0 fy  cy] [Y]
[w']   [ 0  0   1] [Z]
u = u'/w',  v = v'/w'

关键在最后那次"除以 w'"。因为 K 的第三行是 [0, 0, 1]，所以 w' == Z == 深度。这次除法就是"近大远小"的数学本体：同样大小的物体，Z 越大除出来的偏移越小。前面所有的矩阵乘法都是线性的、可逆的，只有这一步是非线性的、丢信息的——这也是为什么单目测距是个难题。

函数返回值把 depths 单独拿出来，是因为投影之后深度信息就没了，但后面过滤"框在不在相机后面"必须用它。

render_sample(...) —— 主流程

顺序是：读图 → 取该相机这一帧的 calibrated_sensor 和 ego_pose → 遍历 sample["anns"] → 每个框走一遍上面三步 → 过滤 → 画线 → 写字。

这里有一个容易被忽略的细节：pose = nusc.get("ego_pose", cam_data["ego_pose_token"]) 取的是相机曝光那一刻的自车位姿，而标注是关键帧（LiDAR 时刻）的。两者相差几十毫秒。官方 devkit 也是这么做的——用相机时刻的 ego_pose 补偿了自车运动，但没有补偿其他物体的运动。所以高速场景下你会看到框略微滞后于车辆，这不是你的代码错了。

五、8 个角点与 12 条边

这段我要专门画一下，因为代码里的注释是错的。代码里的注释写的是：

(0, 1), (1, 2), (2, 3), (3, 0),        # 顶面   ← 错，这是车头面
(4, 5), (5, 6), (6, 7), (7, 4),        # 底面   ← 错，这是车尾面
(0, 4), (1, 5), (2, 6), (3, 7),        # 立柱   ← 错，这是沿车长方向的边

从 x_c = l/2 * [1,1,1,1,-1,-1,-1,-1] 就能看出来：前 4 个点的 x 全是 +l/2，所以 0-3 是同一个纵向切面（车头），不是顶面。拓扑连接本身是对的，画出来的框没问题，只是注释会误导读者。你写书的话这里一定要改。

六、回到你的核心问题：为什么要绕这么大一圈

一句话：标注描述的是"世界的事实"，图像记录的是"某个移动观察者在某一瞬间、透过某个镜头看到的样子"。中间这三步，就是把事实翻译成这个观察者的视角。

拆开看，每一步消掉一个"跟物体本身无关的变量"：

反过来想更清楚：如果 nuScenes 把标注直接存成"每路相机的像素框"，那就要存 6 份、每帧重存一次，而且一旦要做 BEV、要做 LiDAR 融合、要做轨迹预测，这套数据完全没法用。存全局系是唯一能同时服务感知、预测、规划的表示——这一点跟你之前理解的 UniAD 里"Query 在统一空间里传递"是同一个设计哲学。

顺带说，这套变换链你已经在 BEVFormer 里见过了。BEVFormer 的 Spatial Cross-Attention 做的正好是反方向：从 BEV 网格点出发，用同样的 ego_pose 和 calib，投影到 6 路图像上去采样特征。所以这段脚本不只是"验证坐标对不对"，它是 BEV 感知的地基。你把这段跑通并且能手推公式，BEVFormer 的投影模块就不再是黑箱。

七、这份代码值得改进的几处

1. 深度过滤有一条是冗余的。

if np.all(corners_cam[2, :] < min_depth): continue   # 这行可以删
...
if np.any(corners_cam[2, :] < min_depth): continue   # 这行已经覆盖了上面

any 条件严格强于 all，第一个判断永远不会独立生效。

2. 每个框都重建一次四元数，浪费。

global_to_camera 内部每次调用都要 Quaternion(...).rotation_matrix，一帧有几十个框就构造几十次同样的矩阵。应该在 render_sample 里预计算一次，或者干脆合成一个 4×4 齐次矩阵：

def make_T(translation, rotation):
    T = np.eye(4)
    T[:3, :3] = Quaternion(rotation).rotation_matrix
    T[:3, 3] = translation
    return T

T_global2cam = np.linalg.inv(make_T(**calib_kw)) @ np.linalg.inv(make_T(**pose_kw))

用齐次矩阵的好处是"逆变换"和"链式复合"都变成了标准矩阵运算，不容易搞错顺序。写教材的话我建议两种都给：先用分步版讲清楚每一步在干什么，再给齐次矩阵版说明工程上怎么写。

3. 跨越相机近平面的框被整个丢掉了。

if np.any(corners_cam[2, :] < min_depth): continue 会把"一半在相机前、一半在相机后"的框直接扔了。近处的大卡车、正在超车的车经常是这种情况。正确做法是做近平面裁剪（把边在 z = min_depth 处截断），但代码量会多不少。至少应该在注释里说明这是简化。

4. 没有按深度排序。

远处的框和近处的框叠在一起时，绘制顺序是随机的（取决于 sample["anns"] 的顺序）。加一次按平均深度降序排序，先画远的后画近的，视觉上会正确很多。

5. 没有按可见性过滤。

nuScenes 的 sample_annotation 里有 num_lidar_pts 和 visibility_token。点数为 0 的框基本是完全遮挡的物体，画出来只会让图变乱。加一句 if ann["num_lidar_pts"] == 0: continue 效果立竿见影。

6. 没有畸变模型。

nuScenes 只给了 camera_intrinsic，devkit 也是按理想针孔处理的。图像边缘的框会有轻微偏差，这是数据集本身的约定，不是 bug，但值得在书里点一句。

验证的最后一道保险：跑完之后拿官方 devkit 的 nusc.render_sample_data(cam_token) 出一张图对照。两张图的框如果重合，说明你的变换链完全正确；如果只有平移偏差，八成是 Rᵀ·(p−t) 写成了 Rᵀ·p−t；如果框歪了但位置对，检查 w, l, h 的顺序。

为者常成，行者常至