激光雷达标定(坐标系转换)

1. 旋转矩阵

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

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

$$\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]=R\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]\text{\hspace{1em}(1)}$$

图1 二维坐标系转换

• 根据旋转角度$\theta$,由上图1可得$x_1$和$y_1$为：
  $$\begin{gathered} x_1=x_{2}cos\theta -y_{2}sin\theta \\ {y}_1=x_{2}sin\theta +y_{2}cos\theta \text{\hspace{1em}(2)} \end{gathered}$$
• 由此可以得出以下的坐标转换矩阵等式：
  $$\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]=\left[\begin{array}{c}cos\theta ,-sin\theta \\ sin\theta ,cos\theta \end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]\text{\hspace{1em}(3)}$$
• 旋转矩阵R即为：
  $$R=\left[\begin{array}{c}cos\theta ,-sin\theta \\ sin\theta ,cos\theta \end{array}\right]\text{\hspace{1em}(4)}$$
• 由二维推广至三维，由右手定则可以想象在上图1中的O点处有一条垂直于XOY平面指向屏幕的的Z轴：
  

图2 航向角转换

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

图3 俯仰角转换

• 由上述公式（6）同理可以推导出绕坐标轴y轴旋转$\beta$（俯仰角pictch）的公式如下:
  $$\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]\text{\hspace{1em}(7)}$$
• 由此可得俯仰角pictch旋转矩阵$R_{pictch}$为：
  $$R_{pitch}=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]\text{\hspace{1em}(8)}$$
  

图4 横滚角转换

• 也可以推导出绕坐标轴x轴旋转$\gamma$（横滚角roll）的公式如下:
  $$\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\gamma & -sin\gamma \\ 0& sin\gamma & cos\gamma \end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]\text{\hspace{1em}(9)}$$
• 由此可得横滚角roll旋转矩阵$R_{roll}$为：
  $$R_{roll}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\gamma & -sin\gamma \\ 0& sin\gamma & cos\gamma \end{array}\right]\text{\hspace{1em}(10)}$$
• 按照不同的顺序对坐标轴进行旋转可以得到不同的旋转矩阵R，R共有六种形式，分别为
  $$\begin{gathered} 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}\text{\hspace{1em}(11)} \end{gathered}$$
• 按照欧拉角的测量方式，每次旋转按照车底盘坐标系的坐标轴进行旋转（即外旋），外旋旋转矩阵是左乘矩阵，即按照X-Y-Z的顺序进行旋转的话，得到的旋转矩阵是$R=R_Z{R}_Y{R}_X$,通过测量的得到的欧拉角的角度值就可以计算出对应的旋转矩阵。

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

---

2. 平移矩阵

已知激光雷达相对于车坐标原点的三维坐标x,y,z，可以得到激光雷达与车坐标系的平移矩阵为：
$$T=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]\text{\hspace{1em}(12)}$$

图5 三维坐标系转换

---

3. 坐标系的转换

根据上述计算得到的旋转矩阵R和平移矩阵T。设车坐标下点的坐标为$(x_c,y_c,z_c)$激光雷达下点的坐标为$(x_l,y_l,z_l)$即最终的变换形式为：
$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=R\left[\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right]+T\text{\hspace{1em}(13)}$$

---

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);
}