作者：小L
来源：公众号@3D视觉工坊
0. 前言
对于Lidar+IMU系统，往往需要标定Lidar与IMU的外参[4]，即从Lidar到IMU的6个位姿参数。ETH开源的lidar_align代码[0]，用于标定Lidar和里程计Odom之间的位姿参数。本文将对代码进行初步介绍，并总结一些使用中可能会遇到的问题。
1. 代码整体一览
1.1 代码结构
代码主要包括：头文件、cpp、以及ROS的launch启动文件等。其中头文件包括：
aligner.h：Lidar和Odom对齐（外参计算）时用到的类
loader.h：从ROS的Bag或CSV格式载入数据的相关函数
sensors.h：主要包括：里程计Odom，以及雷达Lidar相关接口
transform.h：一些SO3变化的计算以及转换，在插值、优化时使用
1.2 方法基本思想
方法本质解决的是一个优化问题，即在外参参数(6DoF)如何选择时，使Lidar采集到的数据转化到Odom系下后，前后两次scan的数据点能够尽可能的重合。
详细一点儿来说，方法将Lidar数据根据当前假设的状态变量（6DoF参数）变换到Odom系下，构成点云PointCloud，之后对每一次scan时的数据，在下一次scan中通过kdtree的方式寻找最近邻的数据点并计算距离，当总距离最小时可以认为完全匹配，即计算的外参参数正确。
1.3 主要流程
代码主要有两部分：载入数据与优化计算。载入数据部分从Bag数据载入雷达的数据(sensor_msgs/PointCloud2)，并从CSV或Bag数据中载入Odom获得的6DoF位置信息。具体的位置信息如何获得将在后面进行介绍。优化计算部分将在第2.2小节详细展开。
2. 详细介绍
2.1 主要数据类型
Odom数据：主要包括两个数据：时间戳timestamp_us_与从当前时刻到初始时刻的变换T_o0_ot_。
Lidar数据：主要包括两个参数：从Lidar到Odom的外参T_o_l_与每次扫描的数据scans_，而每次的扫描scan数据类型主要包括：扫描起始时间timestamp_us_，本次扫描的点云raw_points_，某个点在Odom的变换（用于去畸变）T_o0_ot_，以及相关配置参数等。
Aligner数据：Aligner首先包含了需要优化的数据OptData（其中包括Lidar、Odom等数据），以及相应的配置参数（数据载入路径、初值、优化参数、KNN相关参数等），以及优化计算的相关参数。
2.2 优化过程详细介绍
在载入了Odom和Lidar数据之后，进行优化求解6个位姿参数。主要求解函数为：lidarOdomTransform
Aligner::lidarOdomTransform()
首先进行相关的优化配置。默认优化参数是6个，但可以考虑两个传感器传输造成的时间差，如果考虑这个因素，参数数量将变为7。
优化时，采用NLOPT优化库[3]，默认首先全局优化这三个参数。如果提供的初值与真值相差较大，或完全没有设置初值（默认为全0），则需要进行全局优化获得旋转参数。在局部优化这6个参数，局部优化开始时的初值就是3个为0的平移参数，以及全局优化计算出来的旋转参数。全局优化、局部优化，都是调用的optimize函数。
Aligner::optimize()
在这个函数设置了NLOPT优化的相关参数，包括：是全局优化还是局部优化、优化问题的上下界、最大迭代次数、求解精度以及目标函数等。最重要的是目标函数LidarOdomMinimizer
LidarOdomMinimizer()
这个函数在优化中会不断调用，迭代计算。首先会通过上一时刻的状态，计算新的从Lidar到Odom的变换（这里用到了Transform.h中定义的一些变换），误差是由lidarOdomKNNError函数获得。
lidarOdomKNNError()
这个是一个重载函数，具有两种重载类型。首先调用的是lidarOdomKNNError(const Lidar)，处理的是Lidar的数据，首先根据估计的Lidar到Odom的变化，对完整的scans_数据计算出每次scan时每个点在Odom下的坐标（getTimeAlignedPointcloud函数，相当于点云去畸变），得到一个结合的点云(CombinedPointcloud)，之后从这个点云中寻找每个点的最近邻，在利用另一个重载类型的lidarOdomKNNError(const Pointcloud, const Pointcloud)函数进行计算最近邻误差。
计算最近邻误差时，构建了一个KD-Tree，并行计算kNNError函数，利用pcl库的nearestKSearch函数搜索一定范围（全局优化时是1m，局部优化时是0.1m）的最近邻，计算最近2个点的误差。
小结
优化的目标函数是每次scan的每个点在完整点云中的最近邻的距离，首先通过粗的全局优化估计一部分参数，再局部优化求解精细的6DoF参数。
3. 配置与运行
3.1 安装
首先在安装时需要安装NLOPT：sudo apt-get install libnlopt-dev。之后把代码拷贝到ros的工作空间，使用 catkin_make进行编译。
3.2 编译可能遇到的问题
这个代码是个人编写使用，没有在大多数的ubuntu和ros版本进行测试，所以可能会遇到各种各样的问题。以Ubuntu18与ROS-melodic为例，首先会遇到一个定义冲突的报错：
1. 定义冲突问题
error: conflicting declaration ‘typedef struct LZ4_stream_t LZ4_stream_t’ typedef struct { long long table[LZ4_STREAMSIZE_U64]; } LZ4_stream_t;
这个原因是ROS版本下有两个头文件定义发生冲突，github的issue中给出了两种解决办法，之一是重命名头文件避免冲突：
sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak
sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4.h.bak
sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h
sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h
（详见：https://github.com/ethz-asl/lidar_align/issues/16）
2. 找不到"FindNLOPT.cmake"
By not providing "FindNLOPT.cmake" in CMAKE_MODULE_PATH this project has asked CMake to find a package configuration file provided by "NLOPT", but CMake did not find one. Solutions: Move "NLOPTConfig.cmake" file to src directory.
解决方法：将"\lidar_align\FindNLOPT.cmake"文件移动到工作路径下中的"\src"文件夹下，再次编译即可。
3.3 测试运行
在github的issue中，由于存在准备数据（尤其是Odom数据）有错误的问题，造成运行失败。作者上传了测试数据https://drive.google.com/open?id=11fUwbVnvej4NZ_0Mntk7XJ2YrZ5Dk3Ub 可以运行测试。
3.4 数据准备
Lidar的数据直接是ros中的sensor_msgs/PointCloud2即可，但位置数据需要提供CSV格式或者ROS下的geometry_msgs/TransformStamped消息类型。后者如何获得？如果是IMU直接进行积分就好，但这样积分势必会不准确，作者也在issue中提到没有考虑noise的问题(https://github.com/ethz-asl/lidar_align/issues/5#issuecomment-432232087 )，所以目前看来对IMU进行积分，凑合使用就好。
[1]给出了一种IMU计算Odom的实现。
3.5 数据采集要求
作者在issue和readme中指出，该方法存在的局限性是，必须要求采集数据时系统进行非平面运动，对平移要求不高但要求旋转必须充分。但对数据量、运动范围没有经过严格的测试。这个局限性也限制了不能用于给无人车这种系统标定。
参考资料
[0]. 原版代码github：https://github.com/ethz-asl/lidar_align
[1]. IMU数据计算Odom的实现：https://www.cnblogs.com/gangyin/p/13366683.html
[2]. lidarr_align原理简要介绍：https://blog.csdn.net/miracle629/article/details/87854450
[3]. NLOPT优化库介绍：https://nlopt.readthedocs.io/en/latest
[4]. Lidar和IMU标定需要标什么？https://blog.csdn.net/tfb760/article/details/108532974
本文仅做学术分享，如有侵权，请联系删文。
下载1
在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。
下载2
在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计汇总等。
下载3
在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。
