自动驾驶-nuscenes 3D 映射真实场景分析

请你从一个实际的场景来将上边整个概念串讲起来，如全局原点选哪里，怎么从全局坐标变为自车坐标，然后再到相机、像素，如一个自动驾驶车，从深圳南山腾讯总部，自驾开到深圳福田平安大厦，这个期间 全局原点在哪里，nuscene 又是怎么采集数据的，然后怎么转换从全局系到车身、相机、像素等，请从实际场景中解读，并对关键场景画真实场景图辅助解释。

下面我用一个真实可想象的场景，把之前所有概念完整串起来讲一遍。

场景设定

一辆采集车从深圳南山腾讯总部出发，沿深南大道自驾开到福田平安金融中心（全程约 12 公里）。
我们假设这辆车完全按照 NuScenes 的采集方式工作。

1. NuScenes 实际是怎么采集数据的？

NuScenes 不是把整个城市当成一个超大全局坐标系，而是把长途驾驶切成很多短片段（每个 scene / log 大约 20 秒，行驶 100~200 米）。

每个短片段都有自己独立的「局部全局坐标系」。

关键点：
从腾讯总部到平安大厦，会被切成大概 60~80 个独立的 scene。
每个 scene 的全局原点都是重新人为选定的，通常选在该 scene 起点附近。

2. 全局原点到底选在哪里？（以其中一个 scene 为例）

假设我们现在看其中一个 scene：车刚从腾讯总部东门出来，沿着深南大道往东开了大约 150 米。

这个 scene 的全局原点会选在：

• 离当前行驶区域很近的一个固定点（比如路口某个角、或者地图上一个方便的参考点）
• 目的是让这个 scene 里所有坐标数值都比较小（几十米到一两百米）

如果把整个深圳用一个原点（比如选在市民中心），那坐标会变成几万米，计算精度和可视化都会很麻烦。所以 NuScenes 选择「每个短 scene 一个本地原点」。

3. 完整变换链条（用这个真实 scene 串起来）

我们以这个 scene 里「前方 40 米处有一辆白色轿车」为例，完整走一遍从全局到像素的过程。

步骤 A：全局坐标系下的标注

白色轿车在本 scene 全局坐标系中的位置是：
[
\text{translation} = [42.5,\ 8.3,\ 0.4]\ \text{米}
]
（中心点坐标 + 尺寸 + 朝向四元数）

步骤 B：全局 → 车身（取逆）

此时采集车自己的 ego_pose 告诉你：
「我现在在全局坐标系里的位置和朝向」。

要把白色轿车的点从全局变到「以我自己车身为中心」的坐标系，必须对 ego_pose 取逆。

步骤 C：车身 → 相机（再次取逆）

相机安装在车顶或挡风玻璃后方，它相对车身有一个固定的外参（calibrated_sensor）。

官方给的是「相机在车身坐标系中的位置」，我们要把点从车身变到相机，所以还要再取一次逆。

步骤 D：相机 → 像素（内参 K 投影）

用相机内参矩阵 K 把相机坐标系下的 3D 点投影到图像平面，得到像素坐标 (u, v)。

最终在前视相机图像上，就能画出这个白色轿车的 3D 框。

4. 把整个从南山到福田的过程串起来看

1. 采集阶段：车载 GPS/IMU + 6 个相机 + 1 个 LiDAR 全程录制。数据被切成很多 20 秒的 scene。
2. 每个 scene 处理：
   • 人为选一个靠近该段的本地原点 → 建立局部全局坐标系
   • 用高精度定位把所有帧的 ego_pose 对齐到这个局部全局系
   • 人工 / 半自动标注所有物体的 3D 框（都在局部全局系下）
3. 使用时（你的代码）：
   • 读某个 scene 的标注（全局）
   • 用该帧的 ego_pose 取逆 → 到车身
   • 用相机 calib 取逆 → 到相机
   • 用 K 投影 → 到像素
   • 画在对应相机图像上做验证

即使车从腾讯开到了平安大厦，每个小 scene 的全局原点都是「就近选择」的，互不干扰。这样既保证了局部精度，又避免了超大坐标带来的数值问题。

总结一张总览图

这就是把所有概念放到真实深圳道路上的完整故事。

如果你想继续深入某一个具体环节（比如「某个 scene 的 ego_pose 具体长什么样」或「相机外参在车上的实际安装位置」），告诉我，我再针对那一块画更细的图。

全局坐标系和自车的坐标系

为什么全局坐标系 取逆后 就为自车的坐标系？这个逻辑是什么意思？比如自车 在全局坐标系的位置为 （12， 25， 0.6）， 那么取逆的逻辑是什么，怎么发现要这么做？自车坐标系原点在车上，为什么还要这么搞，不是很明白。

下面用最直白的逻辑 + 你给的具体数字把这个困惑彻底讲清楚。

核心问题重述

你说：

自车在全局坐标系的位置是 (12, 25, 0.6)，
自车坐标系的原点本来就在车上，
为什么还要对 ego_pose 取逆，才能得到「自车坐标系」？

这是很多人最卡的地方。我们一步步拆开。

1. 先分清两件事

• 自车坐标系的原点：永远钉在车上（后轴中心或 IMU 位置）。
• 标注数据给的位置：所有物体（其他车、行人）的坐标是写在全局坐标系里的。

所以问题变成：
「全局里的一个点，在『以我车子为原点』的坐标系里，应该是多少？」

这就是在做坐标变换，不是简单把原点挪一挪那么简单。

2. 用你给的数字举最简单的例子（先忽略旋转）

假设：

• 自车在全局的位置 ( \mathbf{t} = (12,\ 25,\ 0.6) )
• 前方有一辆目标车，它在全局的位置是 ( \mathbf{p}_{global} = (30,\ 25,\ 0.6) )

（两车在同一条水平线上，目标车在自车正东边 18 米处）

如果我们不做任何变换，直接用全局坐标，目标车的坐标是 (30, 25, 0.6)。
但自车坐标系的原点在自己车上，它「看到」目标车应该是相对自己的位置：(18, 0, 0)。

怎么算出来的？
[
\mathbf{p}{ego} = \mathbf{p}{global} - \mathbf{t} = (30-12,\ 25-25,\ 0.6-0.6) = (18,\ 0,\ 0)
]

这就是平移部分的逆变换：先减去自车在全局的位置。

3. 再加上旋转（真实情况更完整）

自车不可能永远正对着全局的 X 轴。它有朝向（四元数或旋转矩阵 R）。

egopose 完整描述的是：
[
\mathbf{p}{global} = R \cdot \mathbf{p}_{ego} + \mathbf{t}
]
（把「车身坐标系里的点」变到全局）

我们现在要反过来：已知 (\mathbf{p}{global})，求 (\mathbf{p}{ego})。

数学上对上面公式两边同时做逆操作：
[
\mathbf{p}{ego} = R^{T} \cdot (\mathbf{p}{global} - \mathbf{t})
]

这就是代码里写的：

points = points - translation          # 减 t
points = R.T @ points                  # 乘 R 的转置（逆）

逻辑本质：
先把点「搬」到以自车位置为新原点，再把坐标系「转正」到自车自己的朝向。

4. 为什么必须这么做？用「人」来比喻

把自车想成你自己：

• 你站在房间里位置 (12, 25)，面朝某个方向。
• 房间里有一把椅子，它的全局位置是固定的。
• 你想知道：「这把椅子在我的左边还是右边？离我多远？」

你不会直接用椅子的全局坐标，而是会：

1. 用自己的位置去减（平移逆）
2. 根据自己当前面朝的方向重新理解左右前后（旋转逆）
   这就是「取逆」的全部逻辑。

5. 完整带数字的例子（包含旋转）

假设：

• 自车位置 ( \mathbf{t} = (12,\ 25,\ 0.6) )
• 自车朝向：绕 Z 轴旋转了 90°（车头朝全局 Y 轴正方向）
• 目标点全局坐标 ( \mathbf{p}_{global} = (12,\ 40,\ 0.6) )（在自车正北方 15 米）

旋转矩阵 R（90°）：
[
R = \begin{bmatrix} 0 & -1 & 0 \ 1 & 0 & 0 \ 0 & 0 & 1 \end{bmatrix}
]

取逆后：
[
\mathbf{p}{ego} = R^{T} (\mathbf{p}{global} - \mathbf{t}) = R^{T} (0,\ 15,\ 0) = (15,\ 0,\ 0)
]

结果是 (15, 0, 0) → 正好在自车正前方 15 米。完全符合「车头朝北时，北方的点变成自己的前方」。

如果不取逆，直接用全局坐标，你永远得不到「相对自己」的正确位置。

6. 一句话总结逻辑

自车坐标系的原点确实在车上，但数据不是用车身坐标给的，而是用全局坐标给的。
要把「别人写在全局本子上的数字」翻译成「以我自己为原点的数字」，就必须对「我在全局的位姿」做逆变换。

这就是全部逻辑。

如果还觉得抽象，我可以再用你给的 (12, 25, 0.6) 配上一张带真实旋转的动态步骤图，你要吗？

四元数（Quaternion）是什么？

四元数是另一种表示旋转的方式，比旋转矩阵更紧凑、更稳定。

在代码里我们通常会立刻把它转成旋转矩阵再用：

from pyquaternion import Quaternion
q = Quaternion(rotation)          # 四元数
R = q.rotation_matrix             # 变成 3×3 旋转矩阵

下面用最直白的方式讲清楚这句话的意思，以及四元数和旋转矩阵 R到底是什么。

这个词在数学和物理里很常见，中文叫四元数。
简单来说，它是复数的一种扩展，由一个实部和三个虚部组成。如果你是在看 3D 游戏开发、计算机图形学或者机器人控制相关的资料，经常会遇到它，主要用来平滑地表示和计算三维空间里的旋转，能完美避开“万向节死锁”这种坑。

1. “它有朝向”是什么意思？

自车不只是停在某个位置 ((12, 25, 0.6))，它还面朝某个方向。

• 可能车头朝东
• 可能车头朝北
• 也可能斜着开

这个「面朝哪个方向」就叫做朝向（orientation / heading）。

在坐标变换里，光知道位置（平移）不够，还必须知道朝向（旋转），才能正确把全局点变成「相对我自己」的点。

2. 旋转矩阵 R 是什么？

旋转矩阵 R 是一个 (3 \times 3) 的数字表格，专门用来描述「怎么转」。

它的作用很简单：

把一个点绕某个轴旋转一定角度后，新的坐标是多少。

最常见的例子（绕 Z 轴旋转，也就是水平转向）：

• 转 (0^\circ)（车头朝东，假设全局 X 是东）：
  [
  R = \begin{bmatrix}
  1 & 0 & 0 \
  0 & 1 & 0 \
  0 & 0 & 1
  \end{bmatrix}
  ]

• 转 (90^\circ)（车头朝北）：
  [
  R = \begin{bmatrix}
  0 & -1 & 0 \
  1 & 0 & 0 \
  0 & 0 & 1
  \end{bmatrix}
  ]

代码里用的就是这个矩阵：

points = R.T @ points   # 取逆就是转置

3. 四元数（Quaternion）是什么？

四元数是另一种表示旋转的方式，比旋转矩阵更紧凑、更稳定。

它由 4 个数字组成：
[
q = [w,\ x,\ y,\ z]
]

• (w)：实部（和旋转角度有关）
• (x, y, z)：虚部（和旋转轴有关）

NuScenes 里 ego_pose["rotation"] 存的就是这个四元数。

为什么用四元数而不是直接用角度？

1. 避免万向节锁（Gimbal Lock）问题
2. 插值更平滑（动画、轨迹平滑时很有用）
3. 存储只需要 4 个数，而旋转矩阵要 9 个数
4. 计算旋转时效率高、数值更稳定

在代码里我们通常会立刻把它转成旋转矩阵再用：

from pyquaternion import Quaternion
q = Quaternion(rotation)          # 四元数
R = q.rotation_matrix             # 变成 3×3 旋转矩阵

4. 四元数和旋转矩阵的关系

它们是同一种旋转的两种写法，可以互相转换。

NuScenes 选择存四元数，用的时候再转成矩阵。

5. 用一个完整小例子串起来

假设自车：

• 位置 ( t = (12, 25, 0.6) )
• 朝向：绕 Z 轴转了 (90^\circ)（车头朝北）

对应的四元数大约是：
[
q = [0.7071,\ 0,\ 0,\ 0.7071]
]

对应的旋转矩阵就是上面那个 (90^\circ) 的矩阵。

此时如果全局有一个点在自车正北方 15 米，经过：

1. 减去 ( t )
2. 乘以 ( R^T )

就会变成自车坐标系下的 ((15, 0, 0)) —— 正好在自己正前方。

一句话记住

• 朝向 = 车头指向哪里
• 旋转矩阵 R = 用 (3\times3) 数字表精确描述这个朝向
• 四元数 = 用 4 个数字更优雅地描述同一个朝向（NuScenes 实际存储的方式）

两者本质一样，只是表达形式不同。代码里看到 Quaternion(...).rotation_matrix 或 .T，就是在处理这个「朝向」。

需要我再用更简单的 2D 动画式步骤（只转平面）把四元数数字怎么对应到角度讲一遍吗？

旋转矩阵给什么是 3 x 3 的矩阵?

上边的旋转矩阵给什么是 3 x 3 的矩阵，四元数为什么是用 [w,x, y, z] 表示，怎么来的，请解释清楚

下面把两个问题讲透：为什么旋转矩阵一定是 3×3？ 以及 四元数为什么是 [w, x, y, z]，它到底是怎么来的？

一、为什么旋转矩阵是 3×3？

三维空间中的一个点，用三个数字表示：
[
\mathbf{p} = \begin{bmatrix} x \ y \ z \end{bmatrix}
]

旋转是一种线性变换：把一个 3 维向量变成另一个 3 维向量，而且要保持长度不变、角度不变。

任何把 3 维向量变成 3 维向量的线性变换，都可以用一个 3 行 3 列的矩阵来表示：
[
\mathbf{p}' = R \mathbf{p}
]
其中 ( R ) 必须是 ( 3 \times 3 )。

这就是最根本的原因：输入是 3 个数，输出也是 3 个数，所以矩阵必须是 3×3。

额外要求（保证它是“纯旋转”而不是拉伸或镜像）：

• ( R^T R = I )（正交）
• ( \det(R) = 1 )（不翻转）

二、四元数为什么是 [w, x, y, z]？它是怎么来的？

1. 历史与数学来源

四元数是 1843 年爱尔兰数学家汉密尔顿（William Rowan Hamilton）发明的。

他原本想把复数从 2 维扩展到 3 维，但怎么也找不到合适的乘法规则。有一天他突然意识到：必须扩展到 4 维才行。

于是定义了四个单位：
[
1,\quad i,\quad j,\quad k
]
满足：
[
i^2 = j^2 = k^2 = ijk = -1
]

任何一个四元数就写成：
[
q = w + x\,i + y\,j + z\,k
]
也就是我们常写的 [w, x, y, z]。

2. 为什么能表示 3D 旋转？（最关键的推导）

任意一个 3D 旋转，都可以用「绕某个轴转某个角度」来描述（轴角表示）：

• 旋转轴：单位向量 ( \mathbf{u} = (u_x, u_y, u_z) )
• 旋转角度：( \theta )

把这个轴角信息「塞」进四元数的方法是：
[
\begin{align}
w &= \cos\frac{\theta}{2} \
x &= u_x \sin\frac{\theta}{2} \
y &= u_y \sin\frac{\theta}{2} \
z &= u_z \sin\frac{\theta}{2}
\end{align}
]

所以四元数自然就变成了四个数 [w, x, y, z]。

为什么是 θ/2？
这是为了让两个四元数相乘时，角度能正确相加（半角公式），并且保证它是单位四元数（长度为 1）。

3. 直观理解四个数的含义

当 θ = 0（不转）时，q = [1, 0, 0, 0]
当绕 Z 轴转 90° 时，q ≈ [0.707, 0, 0, 0.707]

三、对比总结

两者完全等价，可以互相转换。NuScenes 存四元数是因为更紧凑、数值更稳定；真正做矩阵乘法时再转成 3×3 旋转矩阵。

这就是它们「为什么是这个样子」的完整原因。

为者常成，行者常至