传感器标定 —— lidar_align

用了几天的时间看了下lidar_align项目的源码，现在把整个项目的思路及个人理解整理成一篇博客，供大家参考，如有错误之处，还希望不吝赐教。

移动机器人实现智能化，需要配备多种传感器，比如定位用的GPS，测量姿态用的IMU，测距传感器激光雷达等。多传感器的配合使用往往会涉及到两个较为棘手的问题：一个多传感器数据的时间同步；另一个是多传感器数据的数据融合。而在多传感器数据融合时，首先解决的问题就是坐标系对齐，即将不同坐标系下的测量数据，转换到相同的坐标系下，然后再进行后序数据的处理。

为了获得较好的视野，测距传感器一般放置在机器人位置较高的地方，基于不同的感兴趣区域，还会调整相应的传感器姿态。根据传感器的测距原理，每个测距传感器上都会固连着一个传感器坐标系S，测距传感器基于周围环境测量得到的点云信息，均以此坐标系为参考，而坐标系的具体情况，可以参照厂家给出的说明文档。

为了测量机器人的整体姿态，IMU则放置在机器人的几何中心位置，它会实时测量载体的姿态变化。与测距传感器一样，IMU上也固连着一个虚坐标系，我们一般将此坐标系称为载体坐标系V，IMU测量得到的加速度、角速度、角度等信息都是基于此坐标系的，此坐标系的具体情况可以参考厂家给出的说明文档。

为了完成位姿与视觉信息的融合，需要将点云信息转换到载体坐标系V下，那就需要测量载体坐标系V到传感器坐标系S的转换矩阵T。lidar_align项目给我们提供了一种测量转换矩阵T的解决方案，现将实现思路介绍如下。

为了更好的说明问题，我会做一些理想化或着不太合理的假设。比如，假设测距传感器基于周围环境而获取到的点云信息整体上成矩形，如下图所示，图片是我用word画的一个矩形，然后填充的纹理，看起来像是点云，见谅。

 

如我们准确的获得了由载体坐标系V到传感器坐标系S的转换矩阵T，那么，当机器人沿着直线向前移动时，将点云转换到载体坐标系下后，我们看到某时刻的一系列点云将如下图所示，可以发现前后几帧的点云有很大的区域相互覆盖，那么，对于某一帧点云中的某一个点，与前后几帧点云肯定会有大量重复的点，于是对于这个点，在整体点云中求其K邻近时，K邻近的误差必接近零。由于前后几帧点云中还存在较小区域不会相互覆盖，所以K邻近的误差不会是零。为了进一步的减小K邻近的误差，lidar_align的实现源码中采用了随机采样的方法（均匀分布随机数），即对原始点云进行随机采样，因为前后几帧点云中没有相互覆盖的区域较小，这样我们可以认为采样出来的点，大部分会与重复点，而没有重复点的点将进一步减少，这样对每个点求其K邻近是，误差会进一步减少。

假如我们获取的转换矩阵存在一定的误差，那么，当机器人沿着直线向前移动时，将点云转换到载体坐标系下后，我们看到某时刻的一系列点云将如下图所示。由于误差，导致前后几帧点云中重复区域减少，这样在求每个点的K邻近时，其误差必将增大。

整个项目将点云中每个点的的K邻近误差作为优化目标，采用非线性优化库nlopt进行优化，最终将求取一个转换矩阵T。