从图像坐标系到世界坐标系

1. 相机坐标系到像素坐标系的变换

首先，图像中的像素点是基于相机坐标系获取的，通常在相机捕捉到的图像中，每个像素都可以用图像坐标（u, v）表示。图像坐标和相机坐标系之间的关系可以用相机内参来描述。
相机内参矩阵：相机内参矩阵定义了从相机坐标到像素坐标的变换关系。内参矩阵通常包括焦距 (fx, fy)、主点 (cx, cy) 等参数。
相机坐标到像素坐标的变换：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}fx& 0& cx\\ 0& fy& cy\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$$ 其中，(X, Y, Z) 是相机坐标系中的三维点坐标，(u, v) 是对应的像素坐标。

2. 图像像素坐标到相机坐标系的转换

• 为了将像素坐标转换到相机坐标，需要知道点的深度信息 $Z$ ，也就是该像素点在相机坐标系中的距离。
  如果有深度传感器（如激光雷达）协作，可以直接获取每个像素点的深度信息。 如果没有深度传感器，可以利用图像的深度估计算法估算深度。
  然后，利用以下公式可以将像素坐标 (u, v) 转换到相机坐标系中的三维坐标 (X, Y, Z)：

$$\begin{gathered} X=\frac{(u-cx)\cdot Z}{fx} \\ Y=\frac{(v-cy)\cdot Z}{fy} \end{gathered}$$

3. 相机坐标系到车辆坐标系的变换

• 在Apollo中，车辆的坐标系通常被定义在车辆中心或车辆底部中点位置。要将相机坐标系下的点转换到车辆坐标系，需要知道相机与车辆之间的相对位置和姿态。这可以通过相机外参（包括旋转矩阵和平移向量）来描述。
  旋转矩阵 ® 和 平移向量 (t) 表示了相机坐标系相对于车辆坐标系的旋转和平移。

  相机坐标 $(X,Y,Z)$ 转换到车辆坐标 $(X^{\prime },Y^{\prime },Z^{\prime })$ 的公式为：
  $$\left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\end{array}\right]=R\cdot \left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]+t$$

4. 车辆坐标系到世界坐标系的变换

• 最后，需要将车辆坐标系下的点进一步转换到世界坐标系（通常是地图坐标系或者全球坐标系），这需要知道车辆在地图中的位置和姿态（通常通过GPS或IMU获取）。车辆坐标系到世界坐标系的变换也可以通过一个旋转矩阵和平移向量来表示。
  车辆在世界坐标系下的位置和姿态通常通过Apollo的定位模块来实时更新。

  车辆坐标系到世界坐标系的变换公式类似：
  $$\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]=R_{vehicle\to world}\cdot \left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\end{array}\right]+t_{vehicle\to world}$$

总结

• 经过上述一系列变换，Apollo可以将图像中的一个像素点转换为世界坐标系中的一个点。整个过程概括如下：
  1. 相机内参：将像素坐标转换为相机坐标系。
  2. 深度信息：使用深度传感器或算法获取每个像素的深度。
  3. 相机外参：将相机坐标转换为车辆坐标系。
  4. 定位信息：将车辆坐标系转换为世界坐标系。 这种多级坐标变换流程，使得Apollo可以在世界坐标系下识别并定位图像中的特征点（如车道线、障碍物等），进而进行路径规划和控制。