机器人算法SLAM

机器人算法SLAM

小狼@http://blog.csdn.net/xiaolangyangyang

openslam_gmapping源码分析

代码整体流程

openslam_gmapping核心函数是processScan()，其中主要包括drawFromMotion()、scanMatch()和resample()三个操作。

问题

1. 为什么加入drawFromMotion()函数：

gmapping采用 粒子滤波器 算法，drawFromMotion()函数将所有粒子在odom坐标的基础上加入高斯白噪声，即所谓的放狗过程。

2. scanMatch()做了什么：

scanMatch()函数采用 NDT 算法对当前坐标的激光束和每个粒子各自维护的map进行匹配，对粒子坐标进行前后左右左转右转微调，再给出匹配得分。

3. resample()是怎么进行重采样的：

resample()选出 NDT 算法匹配得分满足要求的粒子，将所有粒子坐标全部移动到这部分粒子上。

4. 全局地图是如何生成的：

gmapping把粒子历史坐标和历史激光数据保存在TNode构成的树形结构中，该树形结构的每一层的节点都是 NDT 算法匹配满足要求的节点（高匹配权重），选择最末端最优粒子（匹配权重最高）相应的TNode节点，逐级向上获取坐标信息和激光信息，生成全局地图。

TNode树形结构图

resample()构建TNode过程

processScan()处理过程（红色为粒子二位空间分布）