给 Cartographer 添加全局重定位功能
新版本的 Cartographer 是没有初始全局重定位的功能的，不过旧版本中有提供一个简单的实现参考，可以看我先前的文章《Cartographer 中的简易全局重定位方法》，但是旧版源码中并没有实际使用这个函数。其实这个函数是不够用的，地图大点都要匹配很久。
下面将会介绍如何在新版 carto 中添加上全局重定位功能，并提供接口供 cartographer_ros 调用。同时也会介绍如何用 carto 自带的线程池加速重定位的计算。
可以先看看原版重定位函数的实现，我删除了一些检查和日志，只关注核心代码：
(资料图片仅供参考)
可以看到，原版的实现真的很简单，就是拿一帧点云，对每一幅 Submap 都做一次全图匹配。这里用的匹配器 FastCorrelativeScanMatcher是 carto 用来做回环检测的匹配器，匹配的成功率还是很不错的，不过匹配后还位姿还会差一点点才能对上，需要再调用一次 CeresScanMatcher2D匹配器进行精确匹配得到正确的位姿。
全局重定位功能实现过程
因为全局重定位需要读取所有的 Submap 数据和最新的点云数据，所以把代码加在 pose_graph_2d模块里是最方便的，cartographer_ros 里也能调用到。
首先就是接口的定义，cartographer_ros 里能通过 MapBuilderBridge访问到 carto 的 MapBuilder实例，而 MapBuilder又能访问到 PoseGraph2D，所以我们只需要在 PoseGraphInterface接口类中添加全局重定位的虚函数，然后在他的派生类中添加对应的实现。
PoseGraphInterface中添加的全局重定位的接口，我们的接口不需要传入点云滤波器，内部直接调体素滤波就行了。
然后到 PoseGraph2D里添加对应的函数。
到这里先介绍一下这个重定位函数的流程，然后逐个部分给出实现代码。
创建线程池；
点云过一次体素滤波；
为每一个 Submap 创建 FastCorrelativeScanMatcher 匹配器，在线程池中执行，并等待所有匹配器创建完成；
遍历每一个匹配器，调用 MatchFullSubmap 方法，记录匹配结果和分数，在线程池中执行，并等待所有匹配完成；
找出匹配得分最高的位姿，创建 CeresScanMatcher2D 匹配器再次匹配得到准确的位姿。
无论是创建匹配器还是全图匹配，计算量都很大，使用线程池来执行这些操作是必要的，我们直接用 carto 本身的线程池，线程池就用 CPU 核心数作为线程数量。
点云滤波也是必须的，一两千个点云的计算量非常大，按 米的参数过一遍体素滤波后只剩几百个点，匹配速度能提升一个数量级，计算结果几乎也没差别。
接着是为每一幅 Submap 创建匹配器，这一步需要在线程池里加速执行。
匹配器创建好之后就是调用全图匹配了，这一步同样需要在线程池里执行。
FastCorrelativeScanMatcher2D 匹配器找出可能性最高的位姿后，就是最后一步使用 CeresScanMatcher2D 匹配器去匹配精确位姿了。
该函数的使用方法也很简单，我的方法是纯定位模式启动后，等加载完 pbstream 地图文件和缓存有效一帧点云数据，通过 MapBuilderBridge暴露的接口调用全局重定位函数，得到有效的匹配位姿后缓存起来，然后析构掉当前的 Node 实例，重新构造一个并使用匹配到的位姿作为初始化位姿来重新开始纯定位模式。可以把这个接口封装成一个RPC请求，grpc 或者是 ros service 都可以，外部调用请求，cartographer_ros 计算返回结果，然后外部再重启纯定位模式。
其实这个方法还是有缺陷的，它只能在机器人停在原地不动的时候才能用，不然等匹配完，机器人已经跑别的地方去了。
标签：
为您推荐
-
人民网杭州9月17日电 （记者孙博洋）9月16日至17日，中国质量（杭州）大会在浙江杭州举行。在16日举行...
2021-09-18