机器人算法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()处理过程（红色为粒子二位空间分布）