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

尼口666

已于 2022-10-28 10:27:55 修改

阅读量1.8k

点赞数 2

CC 4.0 BY-SA版权

文章标签：人工智能机器学习线性代数

于 2022-10-27 12:13:30 首次发布

本文链接：https://blog.youkuaiyun.com/condom10010/article/details/127547059

博客围绕无人车激光雷达BEV图绘制展开，因原始激光雷达点处于车体局部坐标系，需转换到像素坐标系。介绍了二维坐标系变换，包括翻转和平移两步，给出了相应矩阵公式，合并后得到最终坐标变换公式，还提供了C++代码及旋转平移矩阵公式。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

1.动机

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

在这里插入图片描述

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

2.细节

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

2.1翻转

首先需要由O1，xy坐标轴都翻转，变成O2：
在这里插入图片描述
用翻转矩阵表达就是：

$[x2y2]=[−100−1][x1y1]\begin{bmatrix} x2 \\ y2 \end{bmatrix}=\begin{bmatrix} -1& 0\\ 0 & -1\end{bmatrix}\begin{bmatrix} x1 \\ y1 \end{bmatrix}$

2.2 平移

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

用平移矩阵表达就是：
$\begin{bmatrix} x3 \\ y3 \end{bmatrix} =\begin{bmatrix} x2 \\ y2 \end{bmatrix} + \begin{bmatrix} dR \\ dC \end{bmatrix}$

2.3 总结

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

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

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

一般像素坐标不用(x，y)表示，我们换成行和列(R，C)的形式
在这里插入图片描述

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

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

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 旋转平移矩阵公式

$\begin{bmatrix} x2 \\ y2 \end{bmatrix}=\begin{bmatrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{bmatrix}\begin{bmatrix} x1 \\ y1 \end{bmatrix} + \begin{bmatrix} dx \\ dy \end{bmatrix}$
$θ\theta$ 逆时针为正。