多传感融合内外参标定（一）：lidar-imu外参标定工具lidar_align

多传感融合

多传感融合是目前移动机器人、无人驾驶、带Marker AR等前沿人工智能技术的重要解决方案和发展方向之一。由于上述技术都不可避免地涉及在欧式空间定位这一基本感知问题，本系列涉及的传感器就包括了惯性位姿传感器imu/gps/ins、测地激光雷达lidar、摄像机camera等等传感器，会介绍一些标定各传感器内外参数的工具。
多数工具会提供其Github链接，并对其适用情况和局限性进行评述。

传感器内参与外参

标定传感器时必须要遵守的一大原则是：用精度高的测量规范对精度低的测量规范进行校正，例如：量块可以标定千分尺，量块测得值被认为是实际值，而千分尺测得值称为读数。测量时还需要考虑测不准原理，随机误差的存在使得每次测量都无法得到精确的值，但多次测量的读数一般呈正态分布，可用多次测量取平均的方法消除随机误差的影响。
传感器内参校正一般指将传感器读数校正至实际数值处，一般通过修正变换进行。定位相关的传感器通常自身有一个坐标系，在不同传感器数据融合的过程中，数据在不同坐标系下的转换需要使用2个坐标系的外参，通常为旋转矩阵R和平移矩阵T。

lidar-imu外参标定工具lidar_align

不讲废话，先上链接https://github.com/miracle629/lidar_align，项目原链接https://github.com/ethz-asl/lidar_align，感谢ZacharyTaylor及ethz-asl即瑞士苏黎世联邦理工大学------自动驾驶实验室。其实验室对imu与其他传感器融合传感定位建图的理论及项目研究处于世界领先水平，其代表作还包括摄像头与imu融合传感的OKVIS、ROVIO。此次lidar_align主要针对激光雷达与imu（或任意6自由度位姿传感器）外参坐标系标定问题。
首先，激光雷达扫描的每一个点，记录的数据都是根据激光发射和接收的时间差、激光强度差、激光发射的偏航角、俯仰角计算而得，其默认的坐标系是此时刻以激光雷达为原点的坐标系，但激光雷达在整个扫描过程中是运动的，所以如果以运动出发点为世界坐标系原点，获得各时刻激光雷达扫描到的点在世界坐标系下的绝对坐标的方法有2个：
1.$（RT{）}^{-1}$是imu初始坐标系，imu做积分可获得任一时刻相对初始位姿的位姿变换矩阵C，则 $（RT{）}^{-1}C(RT)$表示此时刻激光雷达原点位姿在世界坐标系中的表示，假设某一点在激光雷达坐标系下的表示为S，则其在世界坐标系下的绝对坐标为$（RT{）}^{-1}C(RT)S$。
2.点云扫描前后2帧有很大一部分点云重合，利用点云配准算法，可以估计前后2帧之间的移动转动，从而将每2帧之间的变换$C_i$估计出来，${\prod }_{i=0}^{n-1}{C}_{i}S$表示任一点在世界绝对坐标系下的坐标。
我们使用ICP最近邻迭代算法将2种表示方法描述的同一片点云区域配准，构建并优化最近邻误差，这里不直接对获得的laser坐标系构建优化目标，而对坐标系内的点云构建优化目标，运用了统计误差平均效应，直至收敛，获得RT变换矩阵的估计值，从而获得激光雷达与imu的外参变换矩阵。
用下面这张我手绘的坐标系转换图可以直观理解，laser坐标系用xyz标注，imu坐标系用uvw标注。
这里RT不是指矩阵乘法，而是旋转和平移变换的缩略表示，变换应该为$p1=R\ast p2+t$，用齐次矩阵表示，是一个4乘4的矩阵。

外参标定与laser slam的关系

了解SLAM（同步定位与地图构建）的人应该很容易明白，这两个问题实质上是正问题与反问题的关系，其过程中都涉及定位与建图2个基本任务，甚至其部分代码可以共享。可能引起误差的地方包括：1）imu积分获得运动变换、2）分块点云拼接估计运动、3）最终的点云配准估计外参。此处假设imu和lidar自身的标定已经完成，之后博客里也会涉及单传感器标定的原理和项目。

使用方法与局限

使用lidar_align时主要处理好数据接口问题，建议直接使用rosbag方式读入数据（反正我用另一种方法就没有成功过），对于不发布TransformStamped类型数据的程序，只要稍微改动数据接口的几处函数（全部在loader.cpp文件中）便可以正常使用。据作者使用经验看，最终的误差水平下降到几百或是以下一般表示优化收敛，结果可用。最终还会生成一个.ply文件表示优化过的点云数据三维表示，若与扫描环境一致则结果可信。
此工具的主要局限在于其分块点云配准过于依赖周围环境的约束，适用于移动机器人等可在室内进行标定的机器，利用墙体垂直的强约束进行配准;对于室外无人驾驶汽车，此工具的效果不佳，无人驾驶运动范围较大，与周围约束的建筑物距离较远，约束较弱，由于此工具使用的运动估计算法较为简单，导致室外定位建图效果不佳。

挖坑

作者还进行一项实验，即无人驾驶汽车室外环境数据在ndt正态分布变换下估计运动和建图的效果，发现效果较好，因此得出结论，将lidar_align中定位建图部分换成ndt算法，此工具便可以用于无人驾驶的lidar-imu标定。此处先挖坑，近期在进行一个laser slam项目时考虑顺手填坑。
本人第一次写博，若有不当处请指正，若觉得此项目对各位的工作有帮助，还请顺手在github留颗小星星，感谢各位。