[坐标系转换]车体坐标系 转 像素坐标系

1.动机

在项目中，我们常常需要绘制无人车激光雷达的BEV图。而原始激光雷达的点所处的坐标是车体局部坐标系（简称为vehiclepose）。所以就涉及到一个坐标系的转换，如图所示，由vehiclepose坐标系（记为O1）转换到pixel坐标系（记为O4）。

这里直接给出变换公式：
$$\{\begin{array}{l}R=-x1+dR\\ C=-y1+dC\end{array}$$
可以看到实际上计算非常简单。感兴趣可以看下面的细节。

2.细节

二维坐标系的变换涉及到翻转和平移。下面就分成翻转和平移 2步完成这个转换：

2.1翻转

首先需要由O1，xy坐标轴都翻转，变成O2：

用翻转矩阵表达就是：

$$\left[\begin{array}{c}x2\\ y2\end{array}\right]=\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}x1\\ y1\end{array}\right]$$

2.2 平移

第二步是平移，要把坐标原点平移到像素图的左上角，这里是像素坐标的原点。
需要平移的距离dR是y方向需要平移的距离，dC是x方向需要平移的距离。

用平移矩阵表达就是：
$$\left[\begin{array}{c}x3\\ y3\end{array}\right]=\left[\begin{array}{c}x2\\ y2\end{array}\right]+\left[\begin{array}{c}dR\\ dC\end{array}\right]$$

2.3 总结

将翻转矩阵和平移矩阵合并，就能得到最终的坐标变换公式：
$$\left[\begin{array}{c}x3\\ y3\end{array}\right]=\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}x1\\ y1\end{array}\right]+\left[\begin{array}{c}dR\\ dC\end{array}\right]$$

带入旋转角度，然后化成等式形式为：

$$\{\begin{array}{l}x3=-x1+dR\\ y3=-y1+dC\end{array}$$

一般像素坐标不用(x，y)表示，我们换成行和列(R，C)的形式

由于像素坐标中，行R对应的是x轴，列C对应的是y轴:

$$\{\begin{array}{l}R=-x1+dR\\ C=-y1+dC\end{array}$$

2.4 C++代码：

这里编写了一个函数，输入车体坐标(x,y)，可以返回一个Qvector变量，存储转换后的像素坐标q。

QVector<int> local2img(float x,float y)
{
    QVector<int> q;
    int img_r = pixel_dR - floor(x/grid_size);
    int img_c = pixel_dC - floor(y/grid_size);
    q.append(img_r);
    q.append(img_c);
    return q;
}

• 代码变量注释：
  pixel_dR ：平移量dR
  pixel_dC ：平移量dC
  grid_size：雷达栅格尺寸

3.拓展

3.1 旋转平移矩阵公式

$$\left[\begin{array}{c}x2\\ y2\end{array}\right]=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\left[\begin{array}{c}x1\\ y1\end{array}\right]+\left[\begin{array}{c}dx\\ dy\end{array}\right]$$
$\theta$逆时针为正。