从零开始做自动驾驶定位(十一): 闭环修正

文纯属转载，并认真学习一遍，感谢大佬分享！

注释：文中蓝色文本是自己加上去的

本文章配套源代码地址：https://github.com/Little-Potato-1990/localization_in_auto_driving

测试数据：https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg提取码: n9ys

本篇文章对应的代码Tag为 10.0

代码在后续可能会有调整，如和文章有出入，以实际代码为准

==================================================================================================

一、概述

闭环校正模块是建图功能的最后一个模块了，建图这部分离完工不远了。

闭环模块和其他模块的通信机制我们之前已经设计好，它接收关键帧位姿及对应的GNSS位姿，根据策略做检测，如果检测到满足要求的闭环匹配，那么就把这一对约束关系发送出去，让后端优化模块把这个约束关系作为一个边加入到概率图中就行了

--- 理解这段话的意思，脑中产生一个场景

所以这部分的核心就在于怎么检测了，基本思路就是在历史帧中找离当前关键帧距离最近的关键帧，并匹配对应的点云，如果匹配误差足够小，那么检测就成功了。

但注意，这里只是基本思路，为了提高检测效率和准确度，需要加一些策略。而这些策略就构成了本模块的主要内容。

二、设计思路和代码实现

按上面所说，闭环就是检测历史帧，然后匹配。所以我们自然就把它分成 检测和匹配 两大块来分别分析。

1. 闭环检测

当我们根据当前帧位置从历史帧中查找和它距离最近的关键帧时，会面临哪些问题？不妨罗列一下：

1）如果历史帧产生时间和当前帧时间差太小，那么说明这是一个小闭环，意义不大

2）每产生新的关键帧都对历史帧做一次遍历，太消耗时间

3）如果明知道某一段距离内不会有符合条件的历史帧，那么检测就不必要

为了解决这些问题，自然就有对应的方法

1）设置最小时间差

这里的时间差，严格来讲是关键帧路程差，关键帧的编号是随着车的前进顺序递增产生的，我们设置一个参数diff_num，假如当前帧编号为index，那么寻找匹配的帧应该从编号index-diff_num之前的帧中去查找。diff_num这个值可以通过配置文件设置。

2）为避免频繁检测，每检测一次，就做一次等待

这个等待的帧数，也可以通过配置文件设置，对应的参数为loop_step，即每隔loop_step个关键帧检测一次。

3）根据当前最小距离计算重新计算等待时间

假如本次检测，当前帧和满足条件1）的历史帧中的最短距离是100米，而我们需要的是两米范围内的关键帧，那么我们有理由认为在接下来的至少98米路程内，是不会有满足条件的历史帧的，那么这段距离内就没必要检测了。

通过以上三步，我们基本可以把检测频率控制在一个合适的范围内了。这三步对应的代码在函数  DetectNearestKeyFrame 中，此处不贴代码了。

2. 点云匹配

我们明确知道一点，在点云匹配时，scan-to-map 的匹配是比scan-to-scan的匹配要更精确的，所以我们不应该把当前帧点云和最近距离历史帧点云匹配，而应该以该历史帧为中心，按时间往前和往后各索引几个关键帧，拼接成一个小地图。这个索引的关键帧个数，在配置文件中对应参数为extend_frame_num。---  工程中会有很多的小技巧，也是作者开篇说的意思

另外一点，在前端时，我们为了提高匹配效率，把前端匹配用的点云做了很多的稀疏，而对于闭环匹配来讲，因为频率不高，所以我们可以适当降低稀疏度，让点云稠密一些，这样可以得到更精确的闭环约束。点云滤波提供了两种模式，一种是和前端同样的滤波方法voxel_filter，只不过参数调小了一些，当然也可以在配置文件里自己修改参数。另一种是 no_filter，也就是不滤波的意思，就是直接用原始点云做匹配，我们默认使用的是前者，如果有兴趣，各位可以把其他选项都试一试。

点云匹配结束，会返回一个 匹配误差参数 fitness_score，这个参数越小，代表匹配精度越高，我们设置了一个上限fitness_score_limit，只有小于这个值才建立闭环约束并发送给后端。

三、效果验证

在正式使用时，应该是gnss位置和闭环约束均添加到后端优化中，也就是这两种约束同时使用，我们提供的程序也是默认使用这种模式。

为了单纯评价闭环的效果，各位可以把 back_end.yaml配置文件中的use_gnss先设置为false，即不使用gnss，这样和开环里程计结果一对比，就可以单独评价闭环的效果了。

具体的操作方法是先启动程序和播放bag，然后在数据运行完之后输入指令

rosservice call /optimize_map

这时在slam_data/trajectory文件夹中会生成一个optimized.txt文件，这里存放的是优化后的位姿，该文件夹中还有ground_truth.txt文件，存放对应的gnss位姿，运行如下指令便可以生成对比结果了。

evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz

我把我自己测试的结果贴在这里

可见闭环约束还是消除了很大一部分误差。

另外，在输入上面的优化指令之后，rviz上会显示全局点云地图，从这张地图中可以看到，开环运行时不闭合的地图，在这里闭合了，这也是闭环的重要作用之一。

四、一点思考

其实闭环的原理和实现都不是很难，实现一个基本思路再加一些策略就可以。

这里想讨论的是闭环的作用，以及某些情况下可能发生的要不要加闭环的争论。

想明白要不要，首先得了解它能干什么

所以我们要明白闭环所具备的两个特性：

1）它作为约束，能消除部分累计误差，但不是全部

2）它能够消除点云地图重影

我们就结合这两个特点来回答要不要加闭环的问题。

这个问题要分场景来答：

1. 没有外界先验信息约束的场景

所谓先验信息就是此处的gnss位姿等可以用来做观测的位姿信息。

没有约束信息的往往是室内场景。

对于这个场景，应该不会有太多的争议，它把开环里程计所建的不闭合的地图闭合了，同时消除了部分误差，所以必然是要加的。

这里就同时用到了上面个提到的闭环的两个特性。

当然，在大场景有没有先验信息的情况下，残余误差仍然不可忽视，如果想进一步提高地图精度，往往会加一些landmark之类的约束。

2. 有先验信息参考约束的场景

这种情况可能就会产生一些争议了，比如经常会被质疑，为什么有了gnss还要加闭环？

这个问题的核心就在于gnss做约束还残留哪些问题，以及闭环能不能解决这些问题。

gnss的位置在差分模式下(RTK)，精度确实很高，如果只取固定解，误差能保证在10cm以内，但是我们如果经常用只加gnss、不加闭环方案建图时，会发现在采集车辆重复经过的区域里，某些部分是有重影的。

具体原因也可以合理解释。RTK本身是存在误差的，而且RTK坐标和雷达坐标之间要补偿相对位姿，所以标定误差也会以观测误差的形式存在。而图优化本身是一个平差方法，观测误差必然会在优化结果上有所体现。--- 想一下

虽然这个误差不大，做的好的话，关键帧位置误差往往能控制在10cm以内，差一点也基本能保证在15cm左右。对于一般的自动驾驶任务，这并没有超出指标。但是关键帧误差会在点云地图上带来等量大小的点云重影，因为点云地图是按照关键帧位姿把每一帧点云拼接成的。而且这还只是考虑了位置误差影响，姿态误差同样也会带来重影。

用地图定位时，其实就是把当前帧点云和地图匹配，而带有重影的地图，对定位质量的影响是比较大的，所以需要消除重影。

由于重影是当前帧和历史帧位姿误差造成的，而闭环约束就是当前帧点云和历史帧点云之间重新匹配得到的约束关系，或者可以说它就是直接以重影大小作为观测误差的约束，所以它对重影的消除要有效的多 --- 理解

这时候，关键帧位姿精度可能没有提高，比如当前帧和历史帧可能都有10cm的误差，但是它们二者之间的一致性提高了，导致点云重合度提高了，这个才是关键。

总结一下，对于在有gnss先验信息的情况下还要加闭环的思路，对它的质疑往往给出的理由是，只用gnss约束时定位精度已经很高了，就不再需要闭环去约束了，而我们需要注意到，闭环在这里根本就不是用来提高定位精度的，而是用来消除点云重影的。

所以这里主要起作用的是闭环的第二个特性，而不是第一个，提出这样的质疑往往也是由于对这第二个特性认识不够。

上一篇：从零开始做自动驾驶定位(十): 后端优化

下一篇：从零开始做自动驾驶定位(十二): 前端里程计扩展