单目相机像素坐标系到世界坐标系的转换

世界坐标系 -> 像素坐标系

转换过程可参见这篇文章：FunnyWii's Zone 机器视觉 - 单目相机入门，其中描述了世界坐标系 -> 像素坐标系的转换过程。可以归结为下述公式：

Z_c \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K \begin{bmatrix} R & T \end{bmatrix} \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix}

其中，\text{(}u,v\text{)} 是像素坐标，\text{Z}_c 是该点在相机坐标系下的深度，\text{K} 是相机内参矩阵，\text{R} 和 \text{T} 是外参，\text{(}\text{X}_w,\text{Y}_w,\text{Z}_w\text{)} 是世界坐标系下的三维点。

世界坐标系 -> 像素坐标系的过程描述的是单目相机的成像原理。我们在使用单目相机的过程中，更关心的往往是像素坐标系 -> 世界坐标系的转换过程，也就是还原物体的深度信息。本文基于 CSDN 文章像素坐标转到世界坐标时相机坐标系中的 Zc 值求解_相机坐标系转世界坐标系 zc 怎么求-CSDN 博客。在看完这篇文章之后思路打开，但是也产生了一些新疑问，这些疑问并没有在这篇文章中找到答案。我们先往下看。

像素坐标系 -> 世界坐标系

既然有了上面的公式，那么是不是可以直接进行逆转换，就能得到世界坐标系的点了？

不是。根据公式左边，\text{(}u,v\text{)} 是已知的像素坐标，\text{Z}_c 是未知量；公式右边的 \text{(}\text{X}_w,\text{Y}_w,\text{Z}_w\text{)} 是想要知道的未知量。用 2 个已知量，不能求 3 个未知量。换句话说，仅凭单目图像中的一个像素点，二维求三维在这里行不通。

更准确地说，单个像素反投影到三维空间中，得到的是一条从相机中心出发的空间射线，而不是一个唯一的三维点。只有在已知深度，或者假设这个点落在某个已知平面上时，才能求出唯一交点。

于是像素坐标系 -> 世界坐标系的变换就依赖于一个很重要的假设，即地面假设。如果世界坐标系把地面定义为 \text{Z}_w = 0，并且该像素点对应的空间点位于地面上，那么问题就变成了使用 \text{(}u,v\text{)} 求 \text{(}\text{X}_w,\text{Y}_w\text{)}，二维转二维，可行。

需要注意，如果世界坐标系定义不同，地面不一定是 \text{Z}_w = 0，而应写成对应的平面方程。

简化上述公式，得到（Equation 1）：

\begin{bmatrix} X_w \\ Y_w \\ Z_w \end{bmatrix} = R^{-1} \left( Z_c K^{-1} \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} -T \right)

上述变换可以自行推导，我自己推导了一下，是成立的。进一步简化（Equation 2）：

\begin{bmatrix} X_w \\ Y_w \\ Z_w \end{bmatrix} = Z_c R^{-1}K^{-1} \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} -R^{-1}T

其中，\text{K} 是相机内参，\text{R} 是旋转矩阵，\text{T} 是平移向量。将这些变量用 \text{Mat1} 和 \text{Mat2} 来描述：

Mat1 = R^{-1}K^{-1} \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}, \qquad Mat2 = R^{-1}T

并可以写成（Equation 3）：

\begin{bmatrix} X_w \\ Y_w \\ Z_w \end{bmatrix} = Z_c Mat1 - Mat2

只看 Equation 2 中的 \text{Z}_w，可以得到（Equation 4）：

Z_w = Z_c \cdot Mat1(2,0) - Mat2(2,0)

因此：

Z_c = \frac{Z_w + Mat2(2,0)}{Mat1(2,0)}

当基于地面假设并且 \text{Z}_w = 0 时：

Z_c = \frac{Mat2(2,0)}{Mat1(2,0)}

这就是在地面假设下求 \text{Z}_c 的形式（Equation 5）。

其中，\text{Mat2}(2,0) 代表 \text{Mat2} 的第三项，注意 \text{Mat2} 是一个 \text{3} \times \text{1} 的矩阵。如果基于地面假设，那么 \text{Z}_w 应该是 \text{0}。

几个疑问

当相机坐标系和世界坐标系重合时，得到的世界坐标系为 \text{(0,0,0)}

理论上，世界坐标系和相机坐标系当然可以重合。此时 \text{T} = [0,0,0]^T，那么 \text{Mat2} 和 \text{Mat2}(2,0) 自然也都是 \text{0}（Equation 3）。如果再强行叠加 \text{Z}_w = 0 的地面假设，那么根据 Equation 5 会得到 \text{Z}_c = 0。

问题产生于这个设定本身已经不是正常的像素到地面坐标转换场景。像素坐标反投影得到的是一条空间射线，地面假设本质上是在求这条射线与地面平面的交点。如果相机中心正好落在这个假设平面上，射线与平面的求交就会退化或变得不稳定。

实际应用中，相机通常应该距离地面有非零高度，并且光轴与地面有合适夹角。这样反投影射线才会与地面平面形成合理交点，得到类似 \text{(}\text{X}_w,\text{Y}_w,0\text{)} 的地面坐标。也就是说，在 \text{Z}_w = 0 平面上当然可以存在非零的 \text{(}\text{X}_w,\text{Y}_w,0\text{)} 点，问题不在于 \text{Z}_w 是否非零，而在于射线和平面求交是否退化。

CSDN 文章中应该能看到我的评论，关于旋转矩阵求逆后方可使用的问题。

现在公式上已经没什么问题了，如果要从源码角度实现上述转换，还需要考虑旋转的先后顺序问题。

公式中的 \text{R} 和 \text{T} 是世界坐标系 -> 相机坐标系的旋转矩阵和平移向量。若采用 \text{X}_c = \text{R}\text{X}_w + \text{T}，那么 \text{R}/\text{T} 就是世界坐标到相机坐标的外参，其中 \text{T} = -\text{R}\text{C}_w，\text{C}_w 是相机中心在世界坐标系下的位置。因此，\text{T} 不能简单理解为世界坐标系到相机坐标系原点的平移向量，它和相机中心位置之间还差一个 -\text{R} 的关系。

根据旋转矩阵的定义：

\begin{aligned} \mathcal{M}(\alpha,\beta,\gamma) &= \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} \\ &= \begin{bmatrix} \cos\gamma\cos\beta & -\sin\gamma\cos\alpha+\cos\gamma\sin\beta\sin\alpha & \sin\gamma\sin\alpha+\cos\gamma\sin\beta\cos\alpha \\ \sin\gamma\cos\beta & \cos\gamma\cos\alpha+\sin\gamma\sin\beta\sin\alpha & -\cos\gamma\sin\alpha+\sin\gamma\sin\beta\cos\alpha \\ -\sin\beta & \cos\beta\sin\alpha & \cos\beta\cos\alpha \end{bmatrix} \end{aligned}

其中，\text{(}\alpha,\beta,\gamma\text{)} 分别对应 \text{R}_x、\text{R}_y 和 \text{R}_z。需要注意，根据坐标系正方向不同，\text{roll}、\text{pitch}、\text{yaw} 角和坐标轴的对应关系也会发生变化。

而且，虽然上述公式的顺序是 \text{R}_z\text{R}_y\text{R}_x，但是在列向量右乘的约定下，实际先作用的是最右侧的 \text{R}_x，再作用 \text{R}_y，最后作用 \text{R}_z。不同库可能采用不同的欧拉角顺序、左乘/右乘、内旋/外旋约定，代码实现必须和标定输出保持一致。

先说简单的平移矩阵（向量）\text{T}。在自动驾驶或机器人场景中，一般世界坐标系可以选在车辆后轴中心、车体坐标系原点或其他固定位置，但具体如何定义要和外参标定时的坐标系约定保持一致。

结果的量纲

结果和平移矩阵 \text{T} 使用的量纲相同。你用 \text{m}，那么结果的单位就是 \text{m}；用 \text{mm}，结果就是 \text{mm}。

根据 Equation 5，\text{Z}_w = 0 时，量纲变为 \frac{\text{Mat2}(2,0)}{\text{Mat1}(2,0)}。其中，旋转矩阵 \text{R} 是没有单位的。相机内参 \text{K} 中的 \text{f}_x、\text{f}_y、\text{c}_x、\text{c}_y 以像素为单位，经过 \text{K}^{-1} 后会得到归一化坐标方向。像素坐标 \text{(}u,v\text{)} 和内参中的像素单位相互抵消后，最终世界坐标的尺度由外参平移 \text{T} 或已知平面参数决定。