从零开始做自动驾驶定位(九): 建图系统结构优化
文纯属转载,并认真学习一遍,感谢大佬分享!注释:文中蓝色文本是自己加上去的本文章配套源代码地址:https://github.com/Little-Potato-1990/localization_in_auto_driving测试数据:https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg提取码: n9ys本篇文章对应的代码Tag为 9.0代码在后续可能会
文纯属转载,并认真学习一遍,感谢大佬分享!
注释:文中蓝色文本是自己加上去的
本文章配套源代码地址:https://github.com/Little-Potato-1990/localization_in_auto_driving
测试数据:https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg提取码: n9ys
本篇文章对应的代码Tag为 9.0
代码在后续可能会有调整,如和文章有出入,以实际代码为准
===========================================================================================
一、概述
我们通过前面几篇文章,已经把前端里程计模块的功能实现了,在里程计计算之前做了时间同步、点云畸变补偿等预处理,在里程计计算之后做了实时显示、轨迹精度评测等。这种“之前”和“之后”的描述已经具备了模块划分的概念,即分成了三个模块,只不过我们没有严格按照模块去管理代码。
在建图功能后面的工作里,我们要添加的就是后端优化、闭环检测这两大模块了。
这样我们就有了五个模块,模块数量上来以后,我们就需要有一个更好的代码结构来管理了。
所以本篇文
二、设计思路
上面已经梳理清楚模块的数量和基本功能,那么就需要对每个模块做一个封装,并把代码文件用合适的目录管理好就行。具体封装什么细节,就需要对模块做详细分析了。
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上的显示中,当前点云、位姿与全局地图自然就对应不上,所以要做一次投影。投影所使用的坐标就是看优化时对之前的位姿做了多少修正,这个修正量就是要投影的转换量。
虽然图优化算法和闭环检测算法的具体实现还没加,但这一次改过的架构仍然是可以正常运行的,只不过它实现的仍是里程计功能,区别就是我们内部实现变了,对模块做了划分。运行结果就不往这里贴了,和以前没啥区别。
开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!
更多推荐
所有评论(0)