自动驾驶定位系列教程八_ 建图系统结构优化

自动驾驶定位系列教程八: 建图系统结构优化

一、概述

我们通过前面几篇文章，已经把前端里程计模块的功能实现了，在里程计之前做了时间同步、点云畸变补偿等预处理，在里程计之后做了实时显示、轨迹精度评测等。这种“之前”和“之后”的描述已经具备了模块划分的概念，即分成了三个模块，只不过我们没有严格按照模块去管理代码。

在建图功能后面的工作里，我们要添加的就是后端优化、闭环检测这两大模块了。

这样我们就有了五个模块，模块数量增加了，就需要有一个更好的代码结构来管理。

所以本篇文章的目的就是设计这个代码结构，在我们的系列工作中起到一个承上启下的作用。

附赠自动驾驶学习资料和量产经验：链接

本系列教程涉及完整代码V作者获取

二、设计思路

上面已经梳理清楚模块的数量和基本功能，那么就需要对每个模块做一个封装，并把代码文件用合适的目录管理好就行。具体封装什么细节，就需要对模块做详细分析了。

1. 模块功能详解

我们再详细看一下这五个模块的具体内容。

1）数据预处理

a. 功能

• 接收各传感器信息

• 传感器数据时间同步

• 点云运动畸变补偿

• 传感器信息统一坐标系

b. 输入

• GNSS组合导航位置、姿态、角速度、线速度等

• 雷达点云信息

• 雷达和IMU相对坐标系

c. 输出

• GNSS组合导航位置、姿态

• 畸变补偿后的点云

备注：以上信息均是经过时间同步的，时间戳已保持一致。

2）前端里程计

a. 功能

• 根据点云计算位姿并发送

b. 输入

• 雷达点云

c. 输出

• 里程计累计位姿

3）后端图优化

a. 功能

• 从里程计位姿中提取关键帧

• 根据关键帧位姿、GNSS组合导航约束和闭环检测约束来优化位姿

b. 输入

• 前端里程计位姿

• GNSS组合导航位姿

• 闭环检测相对位姿

c. 输出

• 优化后的位姿

4）闭环检测

a. 功能

• 根据位姿搜索附近历史帧

• 如果有则做匹配

• 如果匹配结果符合要求则建立约束，并发送

b. 输入

• 关键帧对应的GNSS组合导航位姿（因为用开环位姿检测不够准确，尤其大场景）

c. 输出

• 闭环位姿约束

5）显示模块

a. 功能

• 根据优化后的位姿生成点云地图

• 显示点云地图和当前帧点云

b. 输入

• 优化后的历史帧位姿

• 当前帧点云

• 当前帧位姿

c. 输出

• 按优化后的位姿投影之后的当前帧点云

• 按优化后的位姿投影之后的当前帧位姿

• 局部小地图点云

• 全局大地图点云

2. 代码文件管理

通过上面一部分，我们看到第一个模块是建图功能和以后要做的定位功能共有的模块，后面四个模块是建图功能独有的模块。所以我们代码管理的基本思路就是在工程目录先建立文件夹"mapping"，把后四个模块放在文件夹里，第一个模块独立在文件夹外面。

按照之前的思路，为了代码简洁，每个模块内部应该把输入输出功能与核心算法功能分开，而且node文件只是调用模块，所以每个模块就对应了三部分：

1）核心功能

负责实现该模块对应的核心功能，用类封装。放在该模块对应的文件夹下。文件名与功能模块名相同。

2）流程管理

负责该模块的输入输出，并调用核心功能的类。放在该模块对应文件夹下，为区分，文件名以"_flow"结尾。

3）node文件

控制ros循环，调用流程管理的类。我们在工程目录下新建了一个"apps"文件夹，专门用来存放各模块的node文件，为区分，各文件名字以"_node"结尾。

4）配置文件

除了数据预处理功能以外，其他功能模块都配备了配置文件，方便调试。配置文件放在工程目录里的"config"文件夹里。

完成了上面两部分的分析，就可以看各个模块的具体实现了。

三、代码实现

其实模块功能清晰了以后，代码就没太多可说的了。

前端核心算法还是那些，后端图优化部分目前只有基本流程，还没对位姿做优化，所以现在后端模块就是吃什么吐什么，把从前端接收过来的位姿再发给显示模块。

闭环模块暂时没填具体代码，等具体做到那一步的时候再写。

1. 数据预处理

节点文件：apps/data_pretreat_node.cpp

流程管理文件：data_pretreat/data_pretreat_flow.cpp

核心算法文件：data_pretreat/data_pretreat.cpp

没太多可说的，它内部的内容在以前几篇文章都介绍过了，只不过这次重新划分了文件。

2. 前端里程计

节点文件：apps/front_end_node.cpp

流程管理文件：mapping/front_end/front_end_flow.cpp

核心算法文件：mapping/front_end/front_end.cpp

配置文件：config/front_end/config.yaml

以前的代码里有前端里程计的具体实现，此处只做了减法，具体包括：

1）把全局地图生成动能去掉了，放在了其他模块里，因为计算位姿不需要全局地图

2）把关键帧存储成pcd文件的功能去掉了，放在了其他模块里，因为此处只是里程计，不包括关键帧管理

3）groun_truth和odom两个txt文件的存储功能去掉了，放在了其他模块里。因为里程计计算不需要gnss位姿，所以应该和这部分功能解耦。

3. 后端图优化

节点文件：apps/back_end_node.cpp

流程管理文件：mapping/back_end/back_end_flow.cpp

核心算法文件：mapping/back_end/back_end.cpp

配置文件：config/back_end/config.yaml

暂时没对位姿做优化，目前的功能有：

1）接收前端里程计和GNSS组合导航数据，并按时间对齐之后写在txt文件里，方便评测里程计精度

2）从10HZ的里程计位姿中识别出关键帧，并把关键帧位姿和对应的GNSS组合导航位姿发送出去

3）发送历史帧系列位姿，这个功能是为了后面添加图优化功能时使用，每次优化后，把优化后的位姿读取出来，发送给其他模块用。

4. 闭环检测功能

暂时没填写这个功能的代码

5. 显示功能

节点文件：apps/viewer_node.cpp

流程管理文件：mapping/viewer/viewer_flow.cpp

核心算法文件：mapping/viewer/viewer.cpp

配置文件：config/viewer/config.yaml

目前实现的主要功能有：

1）接收优化后的位姿，并根据这个位姿生成全局地图。

由于优化频率低，所以优化生成地图的关键帧可能要比当前全部的关键帧要少很多，具体少多少，可以在优化模块的配置文件里设置。之所以只用优化后的位姿建图，是因为没经过优化的位姿不够精确，用来拼接地图会把地图弄出很多重影，所以我们宁愿舍弃。

另一点是，程序会自动检测rviz上是否要接收地图文件，即rviz界面左侧对应的信息是否被勾选，当勾选时，才生成地图并发送。之所以要这么做是因为，地图文件比较大，生成和发送都比较耗时间，在建图过程中不一定需要实时观看全局地图，这样可以加快速度。

如果想保存最后生成的全局点云地图，可以输入下面的指令

rosservice call /save_map

它会在工程目录的slam_data/map文件夹下生成"map.pcd"文件，这就是地图文件。

当然，地图的生成目录也可以在配置文件中自己指定。

2）接收当前帧点云和位姿，按优化后的位姿投影并发送。

我们接收到的点云是预处理那一步产生的点云，位姿是优化前的里程计位姿，当后端做过图优化之后，如果不投影，那rviz上的显示中，当前点云、位姿与全局地图自然就对应不上，所以要做一次投影。投影所使用的坐标就是看优化时对之前的位姿做了多少修正，这个修正量就是要投影的转换量。

虽然图优化算法和闭环检测算法的具体实现还没加，但这一次改过的架构仍然是可以正常运行的，只不过它实现的仍是里程计功能，区别是内部实现变了，对模块做了划分。运行结果就不往这里贴了，和以前没啥区别。

完整教程联系作者获取