3D激光里程计其三：点云畸变补偿

Reference:

1. 深蓝学院-多传感器融合

1. 产生原因

一帧点云：通常指雷达内部旋转一周扫描得到的点的集合。
优点：有足够数量的点云才能进行匹配，且一周正好是周围环境的完整采集。
缺点：每个激光点的坐标都是相对于雷达的，雷达运动时，不同激光点的坐标原点会不同。

1）平移导致的畸变(转一圈是需要时间的，过程中雷达可能是运动的)
虚线圆圈为真实物体；红色点为激光束打到的位置；红色三角为当前采集时刻雷达的位置；
绿色三角为一帧的坐标原点；绿色点为一帧点云中激光点的坐标

2）旋转导致的畸变(如车辆转向时，采集到的一周并不完整)
假设雷达在顺时针旋转
红色实线为雷达0度坐标轴，绿色实线为一帧点云0度坐标轴

2. 补偿方法

对每个激光点坐标做补偿，补偿量为激光点原点(即当时雷达坐标)相对于该帧起始时刻的变化。

假设一帧点云中，起始时刻雷达的位姿为：$\begin{array}{rl}{T}_0& =\left[\begin{array}{cc}{R}_0& t_0\\ 0& 1\end{array}\right]\end{array}$

第$i$个激光点采集时，雷达的位姿为：$\begin{array}{rl}{T}_i& =\left[\begin{array}{cc}{R}_i& t_i\\ 0& 1\end{array}\right]\end{array}$

第$i$个激光点的坐标为：$P_i=[p_{ix}{p}_{iy}{p}_{iz}{]}^T$

则第个激光点补偿畸变后的坐标应该为： ${\bar{P}}_i=T_0^{-1}{T}_i{P}_i$

上式可以理解为，只需要计算0到$i$时刻，激光雷达的相对旋转和相对平移变化即可。实际上，雷达点云是局部坐标系下的表示，当以0时刻雷达的位姿为基准坐标系时，此时$T_0$即为单位阵，$T_i$即为0到$i$时刻的相对旋转和平移。

此时有：$R_i=\omega d_{time},t_i=V{d}_{time}$

即，只需要知道0到时刻的平均角速度和平均速度即可。