[SLAM] Gmapping Source Reading

1. Motion Model

2. Scan match

Note that the principle of the Scanmatcher::score () function is the Likehood_field_range_finder_model method, referring to the "probabilistic Robot" manuscript P143 page, Scanmatcher::optimize () method, an optimal particle is obtained.

1 //The method here is the Likehood_field_range_finder_model method, referring to the "probabilistic Robot" manuscript P1432InlineDoubleScanmatcher::score (Constscanmatchermap& map,Constorientedpoint& p,Const Double* Readings)Const{3     Doubles=0;4     Const Double* angle=m_laserangles+M_initialbeamsskip;5Orientedpoint lp=p;6Lp.x+=cos (P.theta) *m_laserpose.x-sin (P.theta) *m_laserpose.y;7Lp.y+=sin (P.theta) *m_laserpose.x+cos (P.theta) *m_laserpose.y;8lp.theta+=M_laserpose.theta;9Unsignedintskip=0;Ten     DoubleFreedelta=map.getdelta () *M_freecellratio; One      for(Const Double* R=READINGS+M_INITIALBEAMSSKIP; r<readings+m_laserbeams; r++, angle++){ Askip++; -Skip=skip>m_likelihoodskip?0: Skip; -         if(*r>m_usablerange)Continue; the         if(Skip)Continue; -Point phit=LP; -Phit.x+=*r*cos (lp.theta+*angle); -Phit.y+=*r*sin (lp.theta+*angle); +Intpoint iphit=Map.world2map (phit); -Point pfree=LP; +pfree.x+= (*r-map.getdelta () *freedelta) *cos (lp.theta+*angle); Apfree.y+= (*r-map.getdelta () *freedelta) *sin (lp.theta+*angle); atpfree=pfree-Phit; -Intpoint ipfree=Map.world2map (pfree); -         BOOLFound=false; -Point BESTMU (0.,0.); -          for(intXx=-m_kernelsize; xx<=m_kernelsize; xx++) -          for(intYy=-m_kernelsize; yy<=m_kernelsize; yy++){ inIntpoint pr=iphit+Intpoint (xx,yy); -Intpoint pf=pr+Ipfree; to             //accessibilitystate s=map.storage (). Cellstate (PR); +             //if (s&inside && s&allocated) { -                 Constpointaccumulator& cell=Map.cell (PR); the                 Constpointaccumulator& fcell=Map.cell (PF); *                 if(((Double)) > M_fullnessthreshold && ((Double) Fcell) <m_fullnessthreshold) { $Point mu=phit-Cell.mean ();Panax Notoginseng                     if(!found) { -bestmu=mu; theFound=true; +}Else Abestmu= (MU*MU) < (BESTMU*BESTMU)?MU:BESTMU; the                 } +             //} -         } $         if(found) $S+=exp (-1./M_GAUSSIANSIGMA*BESTMU*BESTMU);//Gaussian proposal Distribution -     } -     returns; the}

View Code

The Scanmatcher::likelihoodandscore () and Scanmatcher::score () methods are basically consistent. However, the newly obtained particles are calculated q, which is prepared for the subsequent weights calculation.

[SLAM] Gmapping Source Reading