gmapping 学习1

1.里程计运动模型解析和gmapping中代码分析

https://blog.youkuaiyun.com/u013019296/article/details/78265224

 

2.scanmatch 算法分析

https://blog.youkuaiyun.com/m0_37350344/article/details/81159413

https://blog.youkuaiyun.com/m0_37350344/article/details/80329928

scanMatch (在gridslamprocessor.hxx中)
    1.遍历粒子滤波每一个粒子(位置):
        调用optimize优化粒子位置，得到该粒子的最优位置和相应score得分
        如果该得分score>m_minimumScore得分阈值，则更新粒子位置，否则不更新。
        用likelihoodAndScore 似然重新计算粒子权重，这个函数跟score是非常像的
    累加所有粒子的score 和权重
    sumScore+=score;
    it->weight+=l;
    it->weightSum+=l;
     
调用updateTreeWeights，对粒子进行归一化，重新计算每个粒子所在路径轨迹中每个点的总权重
    normalize() 在gridslamprocessor.hxx 中进行粒子归一化计算neff(这一步原理还不是很清楚)
    resetTree();
    propagateWeights();

resample粒子重采样，neff的大小来进行重采样(在gridslamprocessor.hxx)（这个过程不是很理解）
    m_neff<m_resampleThreshold*m_particles.size()是进行重采样
    resampleIndexes 找到需要重采样的粒子(怎么找的？)
    把不需要重采样的粒子删除
    
重采样后，调用updateTreeWeights   
        

优化位置：(在scanmatcher.cpp 中)
ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings)
    1.给定初始点init， 计算该点score得分
    2.计算初始点周围前后左右m_optLinearDelta，左旋转右旋转一定角度m_optAngularDelta后的栅格score得分
      得分=增益*score，其中增益与该点离初始点线距离和角偏离有关，
      增益: odo_gain*=exp(-m_angularOdometryReliability*dth)
            odo_gain*=exp(-m_linearOdometryReliability*drho)
      返回新的位置和score
    

score 计算(在scanmatcher.h 中)
    1.计算一帧激光数据每一个点的得分：
        以激光击中点为中心，向边长为2*kernelSize正方形周围栅格搜索
        (即-kernelSize<=x <=kernelSize, -kernelSize<=y <=kernelSize),即正负3个栅格(15cm)
        找到一个被占用的栅格，且该栅格沿着激光方向前面一个栅格是空闲的，记录该点到击中点距离，
        比较在这个正方形内符合条件的栅格，取离击中点距离最小的值，然后计算score分数
        公式：exp(-1/ sigma *d^2 )
    2.累计在这个位置的一帧激光数据所有点的分数,则为该位置的score
    
    
likelihoodAndScore 计算似然来表示粒子的权重，这个函数跟score是非常像的，(在scanmatcher.h 中)
不同的是这个函数除了计算的得分之外，还计算的似然likelihood  
    1.计算一帧激光数据每一个点的得分：
        以激光击中点为中心，向边长为2*kernelSize正方形周围栅格搜索
        (即-kernelSize<=x <=kernelSize, -kernelSize<=y <=kernelSize),即正负3个栅格(15cm)
        找到一个被占用的栅格，且该栅格沿着激光方向前面一个栅格是空闲的，记录该点到击中点距离，
        比较在这个正方形内符合条件的栅格，取离击中点距离最小的值，然后计算score分数
        公式：exp(-1/ sigma *d^2 )  
    
    2.累计在这个位置的一帧激光数据所有点的分数,则为该位置的score
    
    3.计算似然，exp(-1/ m_likelihoodSigma *d^2 )
    double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
    l+=(found)?f:noHit;
    

normalize 把粒子的权重归一化，同时计算出neff用于判断是否需要重采样(在gridslamprocessor.hxx中)
    计算原理不懂！

 

3.如何计算栅格概率的

   https://blog.youkuaiyun.com/weixin_40863346/article/details/92575794  
第一：找到激光雷达在地图的位置(map坐标系下位置)，在scanmatcher.cpp 中的computeActiveArea和registerScan
    lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
    lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
    lp.theta+=m_laserPose.theta;
    IntPoint p0=map.world2map(lp);
    
第二：找到这一帧激光数据中，击中障碍物的激光点在map地图中的坐标，然后从起点到击中点用bresenham画线算法进行划线，
      从起点到终点中的栅格是空闲的，击中点是有障碍物的，然后用cell.update 更新栅格信息
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
        if (m_generateMap){
      /*去除非法的激光束*/
            double d=*r;
            if (d>m_laserMaxRange||d==0.0||isnan(d))
                continue;
            if (d>m_usableRange)
                d=m_usableRange;
      /*被该激光束击中的点的地图坐标*/
            Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
            IntPoint p1=map.world2map(phit);

      /*bresenham画线算法来计算 激光位置和被激光击中的位置之间的空闲位置*/
      GridLineTraversalLine line;
            line.points=m_linePoints;
            GridLineTraversal::gridLine(p0, p1, &line);
      /*更新空闲位置*/
      for (int i=0; i<line.num_points-1; i++){
                PointAccumulator& cell=map.cell(line.points[i]);
        /*更新前的熵的负数*/
        double e=-cell.entropy();
                cell.update(false, Point(0,0));
        /*加上更新后的熵，即更新后的熵减去更新前的熵，求得这次更新对于熵的变化*/
                e+=cell.entropy();
                esum+=e;
            }
      /*更新被击中的位置 只有小于m_usableRange的栅格来用来标记障碍物*/
            if (d<m_usableRange){
                double e=-map.cell(p1).entropy();
                map.cell(p1).update(true, phit);
                e+=map.cell(p1).entropy();
                esum+=e;
            }
        }    

空闲更新
cell.update(false, Point(0,0));
障碍物更新：
map.cell(p1).update(true, phit);

/*
计算每个击中激光点
如果value 是true时，即该栅格是障碍物点，则会累加该障碍物点的坐标和看到障碍物点的次数n,和该栅格观察次数visits
如果value 是false时，即该栅格是空闲点，则累加观察到该栅格的次数visits
*/
void PointAccumulator::update(bool value, const Point& p){
    if (value) {
        acc.x+= static_cast<float>(p.x);
        acc.y+= static_cast<float>(p.y);
        n++;
        visits+=SIGHT_INC;
    } else
        visits++;
}
/*作用：计算是障碍物栅格的坐标
(即这一个激光点的坐标，由于激光点可能会跳动，因此取均值作为该激光点栅格坐标)
*/
inline Point mean() const {return 1./n*Point(acc.x, acc.y);}

  /*返回被占用的概率=观察是障碍物此时n/总共观察到的次数 */
inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; }

4.高斯分布和最大似然估计

https://zhuanlan.zhihu.com/p/21648507?refer=robotics-learning

 

5.gmapping 粒子自适应重采样

https://blog.youkuaiyun.com/tiancailx/article/details/78590809

对粒子滤波的性能具有重要影响的另一个因素是重采样步骤。在重采样期间，低权值的粒子通常由高权值的采样代替。由于用来逼近目标分布使用的粒子数量是有限的，所以重采样步骤非常重要。重采样步骤也可能把一些好的粒子滤去，随着的进行，粒子的数目会逐渐减少，最后导致粒子耗尽使该算法失效。通常采用有效粒子数来衡量粒子权值的退化程度，即

这里的为粒子的归一化权值。
Doucet等为了减少进行重采样步骤的次数，提出了一种理论判定方法来判定是否需要进行重采样。只有当下降到阈值（，为粒子数）以下时，才进行一次重采样。由于重采样只在需要时进行，进行重采样的次数将大大减少。多次的实验证明了这种方法大大降低了将好粒子滤去的风险

normalize 把粒子的权重归一化，同时计算出neff用于判断是否需要重采样(在gridslamprocessor.hxx中)
    计算原理不懂！
    1. 找到权重最大的粒子
    2.对所有粒子进行高斯分布
            m_weights.push_back(exp(gain*(it->weight-lmax)));
            wcum+=m_weights.back();
    3计算有效粒子数 和 归一化权重
        权重=wi/w
        neff = 1/w*w
    
resample 自适应重采样
    1.当有效粒子数小于阈值进行重采样
    2.删除旧粒子，加入新粒子，resampleIndexes，将粒子位姿以及激光数据更新到局部地图？`registerScan()`
    3.若是无需重采样，则建立一个新节点，并加入到树中