多传感器融合定位三-3D激光里程计其三：点云畸变补偿

Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

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到𝑖时刻的平均角速度和平均速度即可。