机器人算法SLAM
机器人算法SLAM
小狼@http://blog.****.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()处理过程（红色为粒子二位空间分布）