激光数据去畸变

机械激光雷达产生数据原理

  机械激光雷达中内置高速旋转的电机，电机旋转的频率决定了激光雷达产生数据的频率。比如一个fov为360度的激光雷达，每秒旋转一圈，那么激光数据的频率就是1Hz；如果每秒旋转10圈，那么激光数据的频率就是10Hz。而激光雷达在旋转一圈时会根据机械结构设计顺序发射激光，激光遇到障碍发生发射，反射的光线被激光雷达获取后根据光发射和接收的时间差计算出被测障碍物的距离。测距公式如下，c表示光速。
$$d=\frac{1}{2}c(t_{recv}-t_{send})$$
  这里需要重点说明的是激光雷达电机旋转一圈会发射上千束激光。发射的激光束越多，激光的角分辨率就越高。比如常用的即机械激光雷达VLP，其激光数据的频率为10Hz或者20Hz，但是每秒钟发射的激光束数量 （专业术语是：采样率）是固定的。那么就会得到这样的结果，激光电机的转速越高，那么激光数据的角度分辨率就越低；反之，激光电机转速越低，激光数据的角度分辨率就越高。

为什么要去畸变

  如上所述，在实际使用中比如机器人，无人车等，这些搭载激光雷达的平台大多情况下是运动的。激光雷达产生激光数据的同时机器人也在运动，但是机器人输出定位数据的频率没有那么高，普通也就10Hz左右。假设激光产生激光数据的频率也是10Hz，而每一帧的激光数据有1000个激光束的情况下，那么就需要插值得到每束激光产生时刻的激光雷达的位姿，这就不可避免的需要对定位输出进行插值去获取每个激光束时刻激光雷达的位姿。还有一点是因为激光雷达产生数据后是每0.1s才会上传一帧数据，1000束激光数据会在一个时刻集中上传，所以需要增加后处理操作把每一束激光雷达产生的时间反算出来。所以计算出每个激光束产生的时间，插值得到激光束产生时刻的激光雷达位姿，这样就可以得到障碍物物的实际位置。
  在ros下使用激光雷达时，激光数据是相对于激光本体坐标系的。画个图看下会比较直观一点。

  如图所示，假设三角形表示激光雷达，在激光雷达上有2个激光发射孔，左右对称分布。这里设定激光雷达从$t_0$到$t_1$时间内激光雷达顺时针旋转角度为$\theta$。在$t_0$时刻激光位置为$P_1$时发射了一束激光，在$t_1$时刻激光位置为$P_2$时又发射了一束激光。激光数据是在激光雷达自身坐标系下表示的，那么静止时刻下2束激光的坐标为[$rcos{\theta }_1$, $rsin{\theta }_1$]，[$rcos{\theta }_2$, $rsin{\theta }_2$]。
在$t_0$时刻到$t_1$时刻激光数据在激光雷达坐标系下的坐标与静止条件下一致的，但是两束激光在惯性坐标系下的在$t_1$时刻坐标明显与惯性系下是不一致的。所以这里就存在一个问题，一帧激光雷达数据包含成百上千个激光束，每个激光束发射时激光雷达本身在惯性系下的坐标是实时变化的。在实际使用中我们要把所有的激光束转换到最后一个时刻时激光雷达在惯性系的位姿$T^{last}$下。
将$li$时刻的激光束数据转换到最后一束激光位姿的公式表示如下，其中$I$表示参考坐标系：
$${T_{last}^I}^{-1}{T}_{li}^I{p}^{li}=T_{li}^{last}{p}^{li}=p^{last}$$
  究其原理，其实就是要确定每束激光发射时激光雷达在参考坐标系下的位姿，就可以把局部坐标系下的坐标转换到目标坐标系下。举个定位中常见例子来表述就是，激光数据与地图进行匹配时，如果知道发射激光束时，激光雷达在地图坐标系下的全局位姿，那么直接使用下面的转换公式就可以得到每束激光在地图坐标系下的坐标。说白了，就是如果能得到每个激光束发射时刻的激光雷达位姿，那么就没必要去畸变了。加入去畸变的原因就是很难准确获取到激光束发射时刻的激光雷达位姿。
$$p^{map}=T_{li}^{map}{p}^{li}$$