[SLAM]GMapping源码阅读

 1 //此处的方法是likehood_field_range_finder_model方法，参考《概率机器人》手稿P143
 2 inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
 3     double s=0;
 4     const double * angle=m_laserAngles+m_initialBeamsSkip;
 5     OrientedPoint lp=p;
 6     lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
 7     lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
 8     lp.theta+=m_laserPose.theta;
 9     unsigned int skip=0;
10     double freeDelta=map.getDelta()*m_freeCellRatio;
11     for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
12         skip++;
13         skip=skip>m_likelihoodSkip?0:skip;
14         if (*r>m_usableRange) continue;
15         if (skip) continue;
16         Point phit=lp;
17         phit.x+=*r*cos(lp.theta+*angle);
18         phit.y+=*r*sin(lp.theta+*angle);
19         IntPoint iphit=map.world2map(phit);
20         Point pfree=lp;
21         pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
22         pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
23          pfree=pfree-phit;
24         IntPoint ipfree=map.world2map(pfree);
25         bool found=false;
26         Point bestMu(0.,0.);
27         for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
28         for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
29             IntPoint pr=iphit+IntPoint(xx,yy);
30             IntPoint pf=pr+ipfree;
31             //AccessibilityState s=map.storage().cellState(pr);
32             //if (s&Inside && s&Allocated){
33                 const PointAccumulator& cell=map.cell(pr);
34                 const PointAccumulator& fcell=map.cell(pf);
35                 if (((double)   )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
36                     Point mu=phit-cell.mean();
37                     if (!found){
38                         bestMu=mu;
39                         found=true;
40                     }else
41                         bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
42                 }
43             //}
44         }
45         if (found)
46             s+=exp(-1./m_gaussianSigma*bestMu*bestMu);//高斯提议分布
47     }
48     return s;
49 }