slam-gmapping 源码阅读2.md

```

Todo: 1）理解路径存储结构，以及粒子结构？

          2）map格式？map.cell.update怎么实现的

          3）重采样的原理？

          4）NDT 计算概率（得分）

接下来涉及到底层:

GridSlamProcessor

• 功能: 实现了基本的基于栅格地图的FastSlam算法,RBPF．每个粒子携带地图以及机器人位姿
• 实现过程：每次收到激光数据，里程计数据，粒子的机器人位姿根据运动模型更新．接下来机器人的位姿用于初始化扫描匹配算法,并根据每个粒子地图进行校正. scanMatcher对每个粒子作局部的优化.
  tip: 当然滤波器只有当机器人运动超过一定阈值才进行更新过程

成员	内容	功能
TNode	位姿,父节点,子节点数,权值	定义了一个节点树,存储轨迹
Particle	地图,位姿,权值,在树中的节点	定义了滤波器的粒子
初始化,设置参数函数	运动模型参数,更新距离,更新周期等.匹配参数
核心处理函数	processTruePos() processScan()
私有函数	scanMatch normalize resample updateTreeWeights	归一化粒子权重 是否进行重采样 更新树的权重

• processScan()

1.对于每一个粒子，根据里程计模型推算机器人当前时刻位姿（t-1位姿，t里程计位姿，t-1里程计位姿） drawFromMotion
2.判断机器人的平移和旋转是否合理
3.处理激光数据（机器人运动一段距离或者到达一定时间）  scanMatch  
4.resample，满足条件重采样，删除旧粒子，产生新粒子,register_scan。不满足重采样条件，就register_scan

• scanMatch(const double* plainReading)

  对于每一个粒子都进行以下操作

 

for every particle Pk in particles

    do

        optimize(Pk) 局部优化粒子位姿 it->pose

        likelihoodAndScore(Pk) 计算粒子权重

        更新粒子权重                       it->weight

        computeActiveArea (Pk) 计算有效区域，更新地图 it->map

 end

• optimize 计算最优的粒子，粒子进行局部优化。
  optimize 调用了 score 这个函数 （计算粒子得分）。原理就参考 《Probabilistic Robot》 一书的P130 页
  
  

  • score 函数:首先计算障碍物的坐标phit，然后将phit转换成网格坐标iPhit 。计算光束上与障碍物相邻的非障碍物网格坐标pfree,pfree由phit沿激光束方向移动一个网格的距离得到，将pfree转换成网格坐标ipfree（增量，并不是实际值） 在iphit 及其附近8个（m_kernelSize:default=1）栅格（pr,对应自由栅格为pf）搜索最优可能是障碍物的栅格。
  • 最优准则： pr 大于某一阈值，pf小于该阈值，且pr栅格的phit的平均坐标与phit的距离bestMu最小。 (算法第七步)
  • 得分计算： s+=exp(-1.0/m_gaussianSigma* bestMu* besMu) 参考NDT算法,距离越大，分数越小，分数的较大值集中在距离最小值处，符合正态分布模型 至此 score 函数结束并返回粒子（currentPose）得分，（算法第八步）
  • 然后回到optimize函数： optimize 干的事就是 currentPose 的位姿进行微调，前、后、左、右、左转、右转 共6次，然后选取得分最高的位姿，返回最终的得分
    如果得分小于最小值，则认为匹配失败，使用里程计位姿

• likelihoodAndSocre 计算粒子的权重（l）

  与score函数基本相似，除了返回s外，还考虑l值。当忽略部分激光束后，则利用m_likelihoodSigma计算f，若匹配成功，l为f,否则 l=noHit

• invalidateActiveArea computeActiveArea 计算可活动区域
  //set up the selective copy of the active area //by detaching the areas that will be updated computeActiveArea 用于计算每个粒子相应的位姿所扫描到的区域 计算过程首先考虑了每个粒子的扫描范围会不会超过子地图的大小，如果会，则resize地图的大小 然后定义了一个activeArea 用于设置可活动区域，调用了gridLine() 函数，Bresenham算法来计算非障碍物格点的集合。

---

• normalize()

  • 归一化 
  • 求有效粒子数
    
    Neff=1/∑(wi)2

• resample(const double* plainReading, int adaptSize, const RangeReading* reading)

1.当有效粒子数小于阈值进行重采样
2.删除旧粒子，加入新粒子，将粒子位姿以及激光数据更新到局部地图？`registerScan()`
3.若是无需重采样，则建立一个新节点，并加入到树中

• registerScan()
  根据Bresenham算法确定激光束扫描过的单元格，所以扫描过的单元格被扫描过的次数加1，激光束末端对应的单元格障碍物标记次数加1，并累加障碍物坐标acc.x和acc.y，则障碍物的。最后单元格是障碍物的概率p = n / visits。

---

• bresenham算法
  bresenham直线算法是用来描绘由两点所决定的直线的算法，它会算出一条线段在n维光栅上最接近的点。源码中的incr2不是很能理解。
  • 算法原理： 由直线的斜率选择在x方向或y方向上每次递增（减）1个单位，决定另一个变量递增（减）量为0或1. 取决于实际直线于最近栅格点的距离，这个距离的最大误差为0.5;
    
    

实现方法：

 

    x=x1,y=y1;

    dx=x2-x1;dy=y2-y1;

    error=dy/dx-0.5;

    for i=1 to dx

         write(x,y,value);

        if (error>=0) then

               y=y+1;

               error=error-1;

        end if

        x=x+1;

        error=error+dy/dx;

        i=i+1;

    end

因为只用到error的符号，可作以下替换Nerror=2* error *dx,这样所有的计算均为整数运算，避免了除法

 

%%start point and end point ,in first xiangxian

x1=0;y1=0;

x2=9;y2=6;

%%plot set

axis([x1 x2 y1 y2]);

axis equal; 

figure(1);

line([x1,x2],[y1,y2],'color','g');

hold on;

grid on;

%%inital state

x=x1;y=y1;

dx=x2-x1;dy=y2-y1;

Nerror=2*dy-dx;

myline=zeros(2,dx);

%%decide the next point

for i=1 : dx

    myline(1,i)=x;myline(2,i)=y;

    plot(x,y,'ro');

    pause(1);

    if (Nerror>=0) 

           y=y+1;

           Nerror=Nerror+2*(dy-dx);

    end 

    x=x+1;

    Nerror=Nerror+2*dy;

end

• 坐标变换
  
  （x,y,theta）表示机器人的位姿，（x_s,y_s）表示激光传感器在机器人坐标系XYr坐标系下的位置。
  （x_zk,y_zk）表示k束激光终点全局坐标。转化公式

---

参考文献

https://blog.youkuaiyun.com/David_Han008/article/details/71524141
https://blog.youkuaiyun.com/tiancailx/article/details/78590809
https://wenku.baidu.com/view/3a67461550e2524de4187e4d.html?from=search
https://blog.youkuaiyun.com/roadseek_zw/article/details/53316177
http://www.cnblogs.com/yhlx125/p/5634128.html