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

Reference:

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

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

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

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

假设一帧点云中，起始时刻雷达的位姿为： T 0 = [ R 0 t 0 0 1 ] \begin{aligned} T_0 & =\left[\begin{array}{cc} R_0 & t_0 \\ 0 & 1 \end{array}\right] \end{aligned} T0​​=[R0​0​t0​1​]​

第 𝑖 𝑖 i个激光点采集时，雷达的位姿为： T i = [ R i t i 0 1 ] \begin{aligned} T_i & =\left[\begin{array}{cc} R_i & t_i \\ 0 & 1 \end{array}\right] \end{aligned} Ti​​=[Ri​0​ti​1​]​

第 𝑖 𝑖 i个激光点的坐标为： P i = [ p i x p i y p i z ] T P_i=[p_{ix}\quad p_{iy}\quad p_{iz}]^T Pi​=[pix​piy​piz​]T

则第𝑖𝑖个激光点补偿畸变后的坐标应该为： P ˉ i = T 0 − 1 T i P i \bar{P}_i=T_0^{-1} T_i P_i Pˉi​=T0−1​Ti​Pi​

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

此时有： R i = ω d t i m e , t i = V d t i m e R_i = \omega d_{time}, t_i = V d_{time} Ri​=ωdtime​,ti​=Vdtime​

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