背景
在最近的项目开展中，我负责速腾M1激光雷达和森云相机SG2-AR0233实现带有回环和重定位功能的融合SLAM算法开发。我自主改进了Scan Context++算法，并根据最新开源的FAST-LIVO2系统特性，开发了高效的回环闭合和重定位功能，这里对Scan Context++算法的改进过程和适配FAST-LIVO2特性的重定位和回环功能逻辑进行记录，下图是项目使用的传感器平台照片。
修改Scan Context++算法
根据我对Scan Context++算法的分析和使用经验发现，Polar Context描述子检测到的回环往往更稳定且更多，而Cart Context描述子由于作用在笛卡尔坐标系中，受到旋转干扰时，Cart Context往往不能检测出回环情况，但Cart Context描述子距离计算往往能得到平移初始值。因此在回环候选寻找阶段，我仅使用PC描述子而不使用CC描述子，这样可以避免由于旋转问题导致CC描述子鲁棒性不高的问题，当PC描述子检测到候选回环时，根据PC描述子给出的旋转初始值估计\(R_{cq}\)，将当前激光雷达帧旋转到候选雷达帧对应位置后，构造当前帧的Cart Context描述子，然后根据构造的当前帧新CC描述子和回环候选的CC描述子进行匹配计算得到平移估计初始值\(t_{cq}\)。修改后的算法流程如下。
Bin编码过程分为PC极坐标编码和CC笛卡尔坐标编码；PC描述子构建过程与Scan Context算法保持一致；- 增强
PC描述子过程对应Scan Context论文中的Root Shift过程 Ring key计算过程与Scan Context算法保持一致；- 数据库
kdtree对应的索引由Ring key描述，而内容保存增强PC描述子以及对应相对平移量、CC描述子和位姿； - 最近回环候选寻找由
kdtree根据Ring Key检索给出； sector key计算估计初始值过程与Scan Context算法保持一致；PC描述子计算实际n与Scan Context算法保持一致；- 根据
PC估计的\(R_{cq}\)计算去旋转CC描述子和当前状态CC描述子； row key和col key计算，其实是行方向平均\(L_1\)范数，列方向平均\(L_1\)范数计算对应的列向量和行向量，然后使用Scan Context++算法描述的最小描述符距离计算得到最小值对应的行偏移索引和列偏移索引；- 使用行偏移索引和列偏移索引计算两个
CC描述符对应邻域内的最小值，然后根据CC描述子的分辨率获得估计的平移向量。而初始的旋转角度由PC描述子距离计算给出。
FAST-LIVO2回环闭合功能和重定位功能
我使用子地图策略实现FAST-LIVO2的回环闭合和全局重定位功能。上图描述的是回环闭合和重定位功能主要包含以下几个方面：
- 子地图结构和保存逻辑（保存时机、保存内容）
- 子地图之间的点云配准和点云与子地图之间的配准逻辑
- 回环闭合功能开始逻辑，对于回环闭合功能，当检测到的回环和当前帧之间不在同一子地图时，回环闭合功能开始
- 全局重定位功能逻辑，全局重定位逻辑主要是在已有子地图保存文件基础上，系统开始时做的重定位逻辑，确定当前帧与回环子地图之间的相对位姿
- 子地图状态更新逻辑，当子地图之间位姿图优化完成后，需要将每个子地图内部对应的点云帧位姿进行更新、平面点更新、平面法向量更新，基于位姿变换更新，更新逻辑比较简单。
子地图结构和保存逻辑
下面图是我截取的FAST-LIVO2的局部地图的管理逻辑，其中区域A和区域B组成了上一时刻的局部地图，区域B和区域C组成这一时刻的局部地图。在区域A析构前，需要对区域A和区域B组成的之前的局部地图进行磁盘永久保存。主要保存内容有：
- 体素内平面中心点，用于子地图之间的配准；
- 体素内平面法向量，用于子地图之间的配准；
- 局部地图内包含的点云帧
id信息和位姿信息，用于scan context++构造的点云之间位姿初值向子地图之间位姿转换； - 局部地图内包含的点云帧对应的
PC描述子、PC增强描述子、CC描述子，用于当前点云帧的scan context++候选回环检测；
子地图之间的配准逻辑和点云与子地图配准逻辑
当回环闭合开始时，当使用改进的scan context++算法获取数据库回环候选的点云帧后，根据改进的scan context++算法，可以拿到初始的描述当前点云帧和回环候选点云帧之间的旋转矩阵和平移向量，将这部分相对位姿转换到对应的两个子地图下。使用双向映射的点到面的ICP算法，实现子地图之间的配准逻辑。
当SLAM系统开始时，需要寻找当前点云在之前地图中的位姿信息，当使用改进scan context++算法获取到候选回环点云后，加载子地图并构建点到面的ICP问题，并使用huber核函数估计当前帧与候选回环子地图的相对位姿关系。
Comments
有什么问题吗，有任何问题欢迎你在下面评论留言或者邮件联系我！