激光雷达标定(坐标系转换)-Toy模板网

这篇具有很好参考价值的文章主要介绍了激光雷达标定(坐标系转换)。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

1. 旋转矩阵

由于激光雷达获取的点云数据的坐标是相对于激光雷达坐标系的，为了使车最终得到的点云数据坐标是在车坐标系下的，我们需要对点云中每一个点的坐标进行坐标转换。
首先是需要对坐标系进行旋转变换，先以二维平面的单位向量坐标转换为例，假设两坐标系中的旋转矩阵为R，旋转角度为 $\theta$ ,点P在 $x_1oy_1$ 坐标(车坐标系)下的坐标为 $x_1,y_1)$ ；点P在 $x_2oy_2$ 坐标（激光雷达坐标系）下的坐标为 $x_2,y_2)$ ,已知点在激光雷达坐标系下的坐标 $x_2,y_2)$ ，可以由以下的坐标系的转换关系得到 $x_1,y_1)$ ：

旋转矩阵R:将点P在旋转后的坐标系下的坐标转换为旋转前的坐标系下的坐标

$\begin{bmatrix} x_1\\y_1 \end{bmatrix} =R\begin{bmatrix} x_2\\y_2 \end{bmatrix} \tag{1}$
激光雷达标定(坐标系转换)

图1 二维坐标系转换

根据旋转角度 $\theta$ ,由上图1可得 $x_1$ 和 $y_1$ 为：
$x_1=x_2cos\theta-y_2sin\theta\\ y_1=x_2sin\theta+y_2cos\theta \tag{2}$
由此可以得出以下的坐标转换矩阵等式：
$\begin{bmatrix} x_1\\y_1 \end{bmatrix} =\begin{bmatrix} cos\theta,-sin\theta\\ sin\theta,cos\theta\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2 \end{bmatrix} \tag{3}$
旋转矩阵R即为：
$=\begin{bmatrix} cos\theta,-sin\theta\\ sin\theta,cos\theta\\ \end{bmatrix} \tag{4}$
由二维推广至三维，由右手定则可以想象在上图1中的O点处有一条垂直于XOY平面指向屏幕的的Z轴：

图2 航向角转换

那么上述的二维空间内的坐标旋转便可推广至三维空间中绕z轴的旋转，旋转角度 $\theta$ 便是欧拉角中的航向角（也称偏航角yaw），由于旋转前后z轴没有发生变换，上述公式（3）可以写为以下形式：
$\begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix} =\begin{bmatrix} cos\theta&-sin\theta&0\\ sin\theta&cos\theta&0\\ 0&0&1\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2\\z_2 \end{bmatrix} \tag{5}$
由此可得航向角（也称偏航角yaw）旋转矩阵 $R_{yaw}$ 为：
$R_{yaw}=\begin{bmatrix} cos\theta&-sin\theta&0\\ sin\theta&cos\theta&0\\ 0&0&1\\ \end{bmatrix} \tag{6}$

图3 俯仰角转换

由上述公式（6）同理可以推导出绕坐标轴y轴旋转 $\beta$ （俯仰角pictch）的公式如下:
$\begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix} =\begin{bmatrix} cos\beta&0&sin\beta\\ 0&1&0\\ -sin\beta&0&cos\beta\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2\\z_2 \end{bmatrix} \tag{7}$
由此可得俯仰角pictch旋转矩阵 $R_{pictch}$ 为：
$R_{pitch}=\begin{bmatrix} cos\beta&0&sin\beta\\ 0&1&0\\ -sin\beta&0&cos\beta\\ \end{bmatrix} \tag{8}$

图4 横滚角转换

也可以推导出绕坐标轴x轴旋转 $\gamma$ （横滚角roll）的公式如下:
$\begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix} =\begin{bmatrix} 1&0&0\\ 0&cos\gamma&-sin\gamma\\ 0&sin\gamma&cos\gamma\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2\\z_2 \end{bmatrix} \tag{9}$
由此可得横滚角roll旋转矩阵 $R_{roll}$ 为：
$R_{roll}=\begin{bmatrix} 1&0&0\\ 0&cos\gamma&-sin\gamma\\ 0&sin\gamma&cos\gamma\\ \end{bmatrix} \tag{10}$
按照不同的顺序对坐标轴进行旋转可以得到不同的旋转矩阵R，R共有六种形式，分别为
$R=R_{yaw}R_{pitch}R_{roll} \ R=R_{yaw}R_{roll}R_{pitch}\\ R=R_{pitch}R_{yaw}R_{roll} \ R=R_{pitch}R_{yaw}R_{roll}\\ R=R_{roll}R_{pitch}R_{yaw} \ R=R_{roll}R_{yaw}R_{pitch} \tag{11}$
按照欧拉角的测量方式，每次旋转按照车底盘坐标系的坐标轴进行旋转（即外旋），外旋旋转矩阵是左乘矩阵，即按照X-Y-Z的顺序进行旋转的话，得到的旋转矩阵是 $R=R_ZR_YR_X$ ,通过测量的得到的欧拉角的角度值就可以计算出对应的旋转矩阵。

外旋（左乘）：每次旋转绕固定轴旋转
内旋（右乘）：每次旋转绕自身旋转后的轴旋转

2. 平移矩阵

已知激光雷达相对于车坐标原点的三维坐标x,y,z，可以得到激光雷达与车坐标系的平移矩阵为：
$T=\begin{bmatrix} x\\y\\z \end{bmatrix} \tag{12}$
激光雷达标定(坐标系转换)

图5 三维坐标系转换

3. 坐标系的转换

根据上述计算得到的旋转矩阵R和平移矩阵T。设车坐标下点的坐标为 $x_c,y_c,z_c)$ 激光雷达下点的坐标为 $x_l,y_l,z_l)$ 即最终的变换形式为：
$\begin{bmatrix} x_c \\ y_c\\ z_c \end{bmatrix} =R\begin{bmatrix} x_l \\ y_l\\ z_l \end{bmatrix} +T \tag{13}$ 文章来源地址https://www.toymoban.com/news/detail-414967.html

4. 坐标转换代码

构造旋转平移矩阵对点云中的点进行转换

//坐标变换将激光雷达坐标系下的点转换到小车坐标系下
void MainWindow::changePoint(pcl::PointCloud<pcl::PointXYZ>::Ptr lidarCloud,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr carCloud,
                 double yaw,double pitch,double roll,double x,double y,double z)
{
    Eigen::Matrix4f transform=Eigen::Matrix4f::Identity();
    Eigen::Matrix4f transformYaw;
    Eigen::Matrix4f transformPitch;
    Eigen::Matrix4f transformRoll;
    
    //航向角
    transformYaw<<cos(yaw),-sin(yaw),0,0,\
            sin(yaw),cos(yaw),0,0,\
            0,0,1,0,\
            0,0,0,1;
    //俯仰角
    transformPitch<<cos(pitch),0,sin(pitch),0,\
            0,1,0,0,\
            -sin(pitch),0,cos(pitch),0,\
            0,0,0,1;
    //横滚角
    transformRoll<<1,0,0,0,\
            0,cos(roll),-sin(roll),0,\
            0,sin(roll),cos(roll),0,\
            0,0,0,1;
            
    //旋转矩阵
    transform=transformRoll*transformPitch*transformYaw;
    
    //平移矩阵
    transform(0,3)=x;
    transform(1,3)=y;
    transform(2,3)=z;
    
    //坐标转换
    pcl::transformPointCloud(*lidarCloud,*carCloud,transform);
}