A Novel 3D LiDAR SLAM Based on Directed Geometry Point and Sparse Frame

系统流程

  DGP-SLAM的流程可以分为三个模块，特征提取模块，局部优化模块和全局优化模块。

特征提取模块

  首先和LeGO-LOAM一样提取线特征和面特征，但是多了一步额外的后处理，根据特征的邻域计算直线方向或者平面法向量之后，保存这样特征点位姿和几何特征的六元组。因为点只会用一次，所以特征点数量变少了。

局部优化模块

• 关键帧选择
    把所有点云分为三类，渲染帧，关键帧和非渲染帧。每隔一定距离选择一帧作为渲染帧，每隔一定数量选择一帧渲染帧作为关键帧。
• 位姿估计
    把最近N帧渲染帧投影到最近的关键帧上作为局部地图。使用点到点的，点到线，点到面和取向之间的匹配误差构建代价函数，使用LM法优化位姿。
• 点传播
    根据上一步估计得到的位姿，采用更严格的判断阈值提取匹配的点到线和点到面的匹配对。根据这些匹配对，对对应渲染帧的线特征或在面特征进行更新。

全局优化模块

  在历史关键帧中查找回环候选帧。使用ICP算法判断回环，然后将回环约束加入Isam2优化框架中对整体位姿进行优化。