Lidar与IMU标定代码实战：lidar_align

作者：小L

本文转载自「计算机视觉工坊」，该公众号重点在于介绍深度学习、智能驾驶等领域，一个小众的公众号。

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_alignFindNLOPT.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视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款