LINS算法的框架与代码分析
作者：chao qin1 haoyang ye christian e. pranata, jun han, shuyang zhang, and ming liu
来源：icra 2020
摘要
lins是以滤波为主的imu、激光雷达紧耦合的激光slam算法。该算法的主要创新点就是用以 ieskf(即迭代误差卡尔曼)为框架，融合 imu 与激光雷达。该算法于发表于 2020 年 icra， 由于网上已经有些同学对算法做了介绍，一些基础的知识本文不再赘述，本文将详细围绕以下两个问题介绍，希望对读者理解算法有所帮助: 1、lins 是如何将激光观测融入滤波框架的? 2、滤波框架与优化框架有何不同?
算法框架
为了讲述通畅还是先介绍下 lins 算法的主要框架，算法流程图如下:
结合代码不难发现，算法的特征提取、以及 mapping 部分都是直接沿用了 lego-loam 算法的，不再赘述。算法的核心创新点为前端激光里程计部分，对应代码的 lins_fusion_node 节点。
理论与代码分析
（一）lins 是如何将激光观测引入滤波框架的? 1、从代码中可以看出，lins只是前端里程计部分采用了滤波框架(即 scan-to-scan 估计帧间位姿)，而后端 scan-to-map部分仍然采用优化方式求解(即用 lm 方法求解位姿)。 2、将点线/点面距离残差作为观测方程引入到代价函数中。 根据论文公式 12，可以看到代价函数分为两个部分，前面是最小化误差状态(即最小化后验与后先验状态之差)，后面的复合函数 f(x)表示点线/面距离，即最小化点线/面距离。也就是说把点线/面距离残差作为观测方程，有了观测方程，然后带入卡尔曼滤波相关公式即可求解。不过，作为一个创新点，作者采用了 ieskf，即迭代误差卡尔曼作为滤波框架。
ieskf 与 eskf 最大的区别就是，在做状态更新时是采用迭代求解的方式计算的，类似高斯牛顿等优化方法求解时需要多次迭代直到收敛。通过迭代的求解的方式，与 eskf 相比可以得到更精确的解。对应论文公式 16。
3、lins 代码实现。lins 中滤波相关的核心部分在 include/stateestimator.hpp 中的 performieskf()函数中实现。
代码中基本是按照论文中的公式 12-18 实现的，比较好读懂。代码中的 findcorrespondingsurffeatures(), findcorrespondingcornerfeatures() 函数即为 costfunction 中的点面/线距离残差计算函数，基本沿用了loam的实现方式，其中jacobiancoffsurfs, jacobiancoffcorns 变量为点面/线距离残差关于点的雅可比矩阵，该过程在之前推送的文章《泡泡点云时空 loam 专题-3】loam 代价函数设计与雅可比求解详细推导(上)》有详细的推导，感兴趣的可以去看下。
代码中，hk_为点面/线距离残差关于误差状态的雅可比矩阵，作者是用 bch 近似求解的，其实含义上与 loam 算法在 odometry，mapping 部分最终求解得到的是一样的，都是点面/线距离关于误差状态的雅可比矩阵，只不过求解方式表达形式不同，loam 中姿态是用欧拉角表示的，lins 是四元数。
performieskf()函数的剩余部分就是论文中公式 15-18 的实现，比较易懂就不赘述了。 （二）滤波与优化有何不同 其实，讨论滤波框架与优化框架的不同是一个相对比较大的问题，为了更具体一些，我们仅就 lins 算法使用的 ieskf 做的滤波的 lio 前端与 lio-sam 等以高斯牛顿或lm优化为框架的 lio 前端做对比(当然严格意义上来说，lio-sam 的前端其实只有预积分，省略了激光里程计，使用 scan-to-map 匹配校正 imu 零偏，但是不影响我们的分析过程)。 1、框架差异。从框架上说，lins 使用的 ieskf 是将所有的状态一起放到滤波框架里估计，而以 lio-sam 为代表的算法前端，是预积分+scan-to-map 算法，其中 scan-to-map 模块使用 lm方法计算帧图位姿，得到计算出的位姿后，输入预积分模块校正imu 零偏。也就是说 imu 零偏的估计与激光匹配位姿计算是分开的，而 lins 使用了 ieskf 将二者当作状态一起估计。 2、理论差异。其实在 1993 年的论文[2]中就已经证明了 ikf 的更新方程与 gauss-newton 理论上是等价的。详细的推导过程可以参考论文，为了方便理解，我们在这里做一个简化的推导。 参照 lins 论文中公式 15-17
当我们使用 gauss-newton 求解待估状态时，通常用到的公式是:
其实从方程的形式上，怎么看这个方程也与 lins 中的公式 16 差别挺大。下面我们尝试尽量直白的解释下 gauss-newton 与 ieskf 更新方程的等价性。 对于一些刚接触slam的同学来说，可能有一个疑问是gauss-newton 求解时很少用到协方差矩阵，但是卡尔曼的更新方程中的卡尔曼增益是需要计算协方差矩阵的，如何理解? 其实 gauss-newton 只是一个公式，当我们设计误差函数时带有协方差矩阵，那么 gauss-newton 求解过程中就会有协方差矩阵的计算，实际上论文[2]中也正是通过这种方式证明的等价性。由于我们实际在用 gauss-newton 求解时基本不考虑协方差矩阵(指点云匹配求解位姿时)，为了更好的理解，我们假设噪声相关的矩阵都为单位矩阵，那么 lins 论文中的公式 15 变为:
那么公式 16 变为
到这一步可以看到跟 gauss-newton 的方程不一致。这个时候要借助一个重要的公式实现转换，即 matrix inversion lemma，即矩阵求逆引理，即如果 a,c 为非奇异矩阵，那么有
那么公式 16 可以变为:
到这里，其实我们看这个形式跟优化的方法已经很像了，但是跟标准的高斯牛顿求解方程还是不一致。现在我们定义如下观测方程:
则其对应的雅可比矩阵为，
因此，
可以看到此时方程出现了我们熟悉的高斯牛顿的公式。因为滤波的框架里面其实是同时考虑状态方程与观测方程，因此使用最小二乘的框架构建的时候需要构建成如上形式。 个人的观点是，如果单从最终的公式来看，使用高斯牛顿求解两帧点云的相对位姿，与 lins 中使用 ieskf 求解结果并不完全相等，因为求解出来的状态更新方程并不是完全一样，但是从理论上说，ieskf 的更新方程可以统一到最小二乘的框架里，因此效果上二者应该是近似的。
算法效果
根据论文提供的实验结果，如果考虑前端+后端的整体处理效果，算法在公园与林间场景表现较好，但是在城区以及港口效果不及 liom。如果只考虑前端激光里程计，算法在除了港口场景外都是最优的，毕竟论文的主要贡献在前端。
总结
本文通过理论分析与代码对比介绍了 lins 算法，其中主要介绍了 lins 如何使用滤波框架做激光 slam，以及优化与滤波的区别。主要是把前端激光里程计部分改为了滤波框架，并且引入了 imu 做紧耦合。同时，我们也推导了滤波的更新方程其实可以由高斯牛顿法推导而来。希望对读者有所帮助，如果错误烦请指出。
我国医疗设备市场规模逐年增长，医用设备市场规模达近2416亿元
基于ESP8266开发板的遥控空调设计案例
Imagination的优化设计套件 (DOK) 可提供显著的硅 PPA 效益，并缩短设计周期时间
发电机的励磁系统有什么用_发电机的励磁电路图
再上新台阶，青海电网新能源单日发电量达到9670万千瓦时
LINS算法的框架与代码分析
容积式压缩机冷水机组
用街景图守卫建筑：深度学习为提高建筑安全铺平道路
时刻关注车载充电器 不可小看其“威力”
单相PFC电路设计
西安交通大学人工智能与机器人研究所公开全球首个五维驾驶场景理解数据集
ios10.3最新消息：有多少人升级iOS10.3只是为了那多出来一两G内存的？
什么是CircleIndicator？CircleIndicator的源码实现
华为2018年财报：全球销售收入7212亿元人民币，同比增长了19.5%
介绍四款远距离无线通信模块
LED显示产品路线图
PHP中数组的使用方法！
安捷伦收购AT4 wireless的测试系统业务
腾讯发力计算机视觉 将推动人工智能迎来质变
Silicon labs：关于Zigbee的解决方案一直在路上
来源：icra 2020
摘要
lins是以滤波为主的imu、激光雷达紧耦合的激光slam算法。该算法的主要创新点就是用以 ieskf(即迭代误差卡尔曼)为框架，融合 imu 与激光雷达。该算法于发表于 2020 年 icra， 由于网上已经有些同学对算法做了介绍，一些基础的知识本文不再赘述，本文将详细围绕以下两个问题介绍，希望对读者理解算法有所帮助: 1、lins 是如何将激光观测融入滤波框架的? 2、滤波框架与优化框架有何不同?
算法框架
为了讲述通畅还是先介绍下 lins 算法的主要框架，算法流程图如下:
结合代码不难发现，算法的特征提取、以及 mapping 部分都是直接沿用了 lego-loam 算法的，不再赘述。算法的核心创新点为前端激光里程计部分，对应代码的 lins_fusion_node 节点。
理论与代码分析
（一）lins 是如何将激光观测引入滤波框架的? 1、从代码中可以看出，lins只是前端里程计部分采用了滤波框架(即 scan-to-scan 估计帧间位姿)，而后端 scan-to-map部分仍然采用优化方式求解(即用 lm 方法求解位姿)。 2、将点线/点面距离残差作为观测方程引入到代价函数中。 根据论文公式 12，可以看到代价函数分为两个部分，前面是最小化误差状态(即最小化后验与后先验状态之差)，后面的复合函数 f(x)表示点线/面距离，即最小化点线/面距离。也就是说把点线/面距离残差作为观测方程，有了观测方程，然后带入卡尔曼滤波相关公式即可求解。不过，作为一个创新点，作者采用了 ieskf，即迭代误差卡尔曼作为滤波框架。
ieskf 与 eskf 最大的区别就是，在做状态更新时是采用迭代求解的方式计算的，类似高斯牛顿等优化方法求解时需要多次迭代直到收敛。通过迭代的求解的方式，与 eskf 相比可以得到更精确的解。对应论文公式 16。
3、lins 代码实现。lins 中滤波相关的核心部分在 include/stateestimator.hpp 中的 performieskf()函数中实现。
代码中基本是按照论文中的公式 12-18 实现的，比较好读懂。代码中的 findcorrespondingsurffeatures(), findcorrespondingcornerfeatures() 函数即为 costfunction 中的点面/线距离残差计算函数，基本沿用了loam的实现方式，其中jacobiancoffsurfs, jacobiancoffcorns 变量为点面/线距离残差关于点的雅可比矩阵，该过程在之前推送的文章《泡泡点云时空 loam 专题-3】loam 代价函数设计与雅可比求解详细推导(上)》有详细的推导，感兴趣的可以去看下。
代码中，hk_为点面/线距离残差关于误差状态的雅可比矩阵，作者是用 bch 近似求解的，其实含义上与 loam 算法在 odometry，mapping 部分最终求解得到的是一样的，都是点面/线距离关于误差状态的雅可比矩阵，只不过求解方式表达形式不同，loam 中姿态是用欧拉角表示的，lins 是四元数。
performieskf()函数的剩余部分就是论文中公式 15-18 的实现，比较易懂就不赘述了。 （二）滤波与优化有何不同 其实，讨论滤波框架与优化框架的不同是一个相对比较大的问题，为了更具体一些，我们仅就 lins 算法使用的 ieskf 做的滤波的 lio 前端与 lio-sam 等以高斯牛顿或lm优化为框架的 lio 前端做对比(当然严格意义上来说，lio-sam 的前端其实只有预积分，省略了激光里程计，使用 scan-to-map 匹配校正 imu 零偏，但是不影响我们的分析过程)。 1、框架差异。从框架上说，lins 使用的 ieskf 是将所有的状态一起放到滤波框架里估计，而以 lio-sam 为代表的算法前端，是预积分+scan-to-map 算法，其中 scan-to-map 模块使用 lm方法计算帧图位姿，得到计算出的位姿后，输入预积分模块校正imu 零偏。也就是说 imu 零偏的估计与激光匹配位姿计算是分开的，而 lins 使用了 ieskf 将二者当作状态一起估计。 2、理论差异。其实在 1993 年的论文[2]中就已经证明了 ikf 的更新方程与 gauss-newton 理论上是等价的。详细的推导过程可以参考论文，为了方便理解，我们在这里做一个简化的推导。 参照 lins 论文中公式 15-17
当我们使用 gauss-newton 求解待估状态时，通常用到的公式是:
其实从方程的形式上，怎么看这个方程也与 lins 中的公式 16 差别挺大。下面我们尝试尽量直白的解释下 gauss-newton 与 ieskf 更新方程的等价性。 对于一些刚接触slam的同学来说，可能有一个疑问是gauss-newton 求解时很少用到协方差矩阵，但是卡尔曼的更新方程中的卡尔曼增益是需要计算协方差矩阵的，如何理解? 其实 gauss-newton 只是一个公式，当我们设计误差函数时带有协方差矩阵，那么 gauss-newton 求解过程中就会有协方差矩阵的计算，实际上论文[2]中也正是通过这种方式证明的等价性。由于我们实际在用 gauss-newton 求解时基本不考虑协方差矩阵(指点云匹配求解位姿时)，为了更好的理解，我们假设噪声相关的矩阵都为单位矩阵，那么 lins 论文中的公式 15 变为:
那么公式 16 变为
到这一步可以看到跟 gauss-newton 的方程不一致。这个时候要借助一个重要的公式实现转换，即 matrix inversion lemma，即矩阵求逆引理，即如果 a,c 为非奇异矩阵，那么有
那么公式 16 可以变为:
到这里，其实我们看这个形式跟优化的方法已经很像了，但是跟标准的高斯牛顿求解方程还是不一致。现在我们定义如下观测方程:
则其对应的雅可比矩阵为，
因此，
可以看到此时方程出现了我们熟悉的高斯牛顿的公式。因为滤波的框架里面其实是同时考虑状态方程与观测方程，因此使用最小二乘的框架构建的时候需要构建成如上形式。 个人的观点是，如果单从最终的公式来看，使用高斯牛顿求解两帧点云的相对位姿，与 lins 中使用 ieskf 求解结果并不完全相等，因为求解出来的状态更新方程并不是完全一样，但是从理论上说，ieskf 的更新方程可以统一到最小二乘的框架里，因此效果上二者应该是近似的。
算法效果
根据论文提供的实验结果，如果考虑前端+后端的整体处理效果，算法在公园与林间场景表现较好，但是在城区以及港口效果不及 liom。如果只考虑前端激光里程计，算法在除了港口场景外都是最优的，毕竟论文的主要贡献在前端。
总结
本文通过理论分析与代码对比介绍了 lins 算法，其中主要介绍了 lins 如何使用滤波框架做激光 slam，以及优化与滤波的区别。主要是把前端激光里程计部分改为了滤波框架，并且引入了 imu 做紧耦合。同时，我们也推导了滤波的更新方程其实可以由高斯牛顿法推导而来。希望对读者有所帮助，如果错误烦请指出。
我国医疗设备市场规模逐年增长，医用设备市场规模达近2416亿元
基于ESP8266开发板的遥控空调设计案例
Imagination的优化设计套件 (DOK) 可提供显著的硅 PPA 效益，并缩短设计周期时间
发电机的励磁系统有什么用_发电机的励磁电路图
再上新台阶，青海电网新能源单日发电量达到9670万千瓦时
LINS算法的框架与代码分析
容积式压缩机冷水机组
用街景图守卫建筑：深度学习为提高建筑安全铺平道路
时刻关注车载充电器 不可小看其“威力”
单相PFC电路设计
西安交通大学人工智能与机器人研究所公开全球首个五维驾驶场景理解数据集
ios10.3最新消息：有多少人升级iOS10.3只是为了那多出来一两G内存的？
什么是CircleIndicator？CircleIndicator的源码实现
华为2018年财报：全球销售收入7212亿元人民币，同比增长了19.5%
介绍四款远距离无线通信模块
LED显示产品路线图
PHP中数组的使用方法！
安捷伦收购AT4 wireless的测试系统业务
腾讯发力计算机视觉 将推动人工智能迎来质变
Silicon labs：关于Zigbee的解决方案一直在路上