筆記和總結(jié)：

void SlamGMapping::startLiveSlam()主要是訂閱話題，主要有
scan_filter_-> registerCallback(boost::bind(&SlamGMapping::laserCallback,this, _1);
這句話就是要對激光雷達(dá)的數(shù)據(jù)進行處理的回調(diào)函數(shù)
接下來我們查看此回調(diào)函數(shù)
SlamGMapping::laserCallback(const sensor_msgs::LaserScan
::ConstPtr& scan)
//這個函數(shù)首先是調(diào)用initMapper(*scan)初始化函數(shù)，
//一些重要參數(shù)的初始化，將slam里的參數(shù)傳遞到 openslam 里 ，
//設(shè)定坐標(biāo)系，坐標(biāo)原點，以及采樣函數(shù)隨機種子的初始化，等等主
//要的調(diào)用有gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_, kernelSize_, lstep_, astep_, iterations_,lsigma_, ogain_, lskip_);   //激光雷達(dá)數(shù)據(jù)匹配的參數(shù)gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);  //設(shè)置更新距離的參數(shù)gsp_->setUpdatePeriod(temporalUpdate_); //設(shè)置更新的時間gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, //xmax_, ymax_, delta_, initialPose); //初始化粒子的大小gsp_->setllsamplestep(llsamplestep_);    //設(shè)置激光雷達(dá)的步長
還有很多就不再意義舉例這個函數(shù)*要轉(zhuǎn)到pf的核心代碼了將調(diào)用processScan 返回的值是一個bool判斷是否更新成功現(xiàn)在進入函數(shù)addScan(*scan, odom_pose)看看用到了哪些函數(shù)，
此函數(shù)的輸入是激光雷達(dá)的數(shù)據(jù)和里程計的信息，很明顯這里面就要
用到了里程計模型，以及傳說中的粒子濾波，通過粒子濾波更新機器
人的位姿。所以這一個函數(shù)是gmapping的核心代碼。
讀取位姿和激光數(shù)據(jù)，返回下面的函數(shù)，處理數(shù)據(jù)
return gsp_->processScan(reading);   //處理激光數(shù)據(jù)也
//就是代碼的核心都在這里  此函數(shù)在gridslamprocessor.cpp文
//件中，結(jié)束，回到laserCallback，還有最優(yōu)一步updateMap1.  進入processScan（reading）就是要對運動模型采樣的函數(shù)，
獲取由運動采樣獲得的采樣位 
pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose); //第一個參數(shù)是輸出，第二個參數(shù)是當(dāng)前位姿，
m_odoPose是上一時刻的位姿
//現(xiàn)在我們就進入drawFromMotion 函數(shù)中查看實現(xiàn)代碼
//具體的實現(xiàn)理論與代碼的分析可查看之前的博客  
//雖然寫的不是很清楚  但是我已經(jīng)盡力了更新位姿就是要計算機器人累計的平移和旋轉(zhuǎn)
// accumulate the robot translation and rotation
OrientedPoint move=relPose-m_odoPose;
move.theta=atan2(sin(move.theta), cos(move.theta));
m_linearDistance+=sqrt(move*move);
m_angularDistance+=fabs(move.theta);
//在從運動模型采樣后  也就是預(yù)測了粒子的位姿之后，
//接下來就是要根據(jù)測量數(shù)據(jù)對當(dāng)前的粒子的位姿進行更新。2.  接下來就有另外一個重要函數(shù)
scanMatch(plainReading);//這個函數(shù)主要是根據(jù)測量
//數(shù)據(jù)與之前的地圖比較更新粒子的位姿 以獲得最優(yōu)粒子位姿在這個函數(shù)中需要計算每個粒子的得分，如果得分低者將會被丟
棄或者權(quán)重下降，也就是為了尋找最優(yōu)粒子。在程序中的順序是scanMatch()此函數(shù)的具體是：GridSlamProcessor::scanMatch()//實現(xiàn)過程是在此函
//數(shù)的具體是：GridSlamProcessor.hxx文件中接下來就是在這里scanMatch()函數(shù)中的函數(shù) 第一個重要的函數(shù)：  
（1）從每一次的激光數(shù)據(jù)中獲取新的位姿。計算最優(yōu)粒子的修正后
的位姿并返回其得分 
score=m_matcher.optimize(corrected, it->map, it->pose, plainReading); //這里的corrected就是根據(jù)測量過得的修
正后的粒子位姿，那么在optimize 調(diào)用了 score 這個函數(shù)計算
粒子得分,具體理論的
//知識的研究詳情查看《概率機器人》書中關(guān)于測量模型的解釋在score 函數(shù)里，首先計算障礙物的坐標(biāo)phit，然后將phit轉(zhuǎn)換成網(wǎng)格坐標(biāo)iPhit
計算光束上與障礙物相鄰的非障礙物網(wǎng)格坐標(biāo)pfree,pfree由phit沿激光束方向移動一個網(wǎng)格的距離得到，將pfree轉(zhuǎn)換成網(wǎng)格坐標(biāo)ipfree（增量，并不是實際值）
在iphit 及其附近8個（m_kernelSize:default=1）柵格（pr,對應(yīng)自由柵格為pf）搜索最優(yōu)可能是障礙物的柵格。
最優(yōu)準(zhǔn)則： pr 大于某一閾值，pf小于該閾值，且pr柵格的phit的平均坐標(biāo)與phit的距離bestMu最小。得分計算：s +=exp(-1.0/m_gaussianSigma*bestMu*besMu) //距離越大，分?jǐn)?shù)越小，分?jǐn)?shù)的較大值集中在距離最小值處，符合正態(tài)分布模型
至此 score 函數(shù)結(jié)束并返回粒子（currentPose）得分，然后回到optimize函數(shù)所以optimize（）的作用就是使用運動模型采樣到的當(dāng)前時刻的位姿 currentPose 進行微調(diào)，也就是傳說中粒子濾波中的更新步驟。獲得的位姿一定很接近正確位姿，所以該函數(shù)只是在之前粒子位姿的基礎(chǔ)上對其進行前、后、左、右、左轉(zhuǎn)、右轉(zhuǎn) 共6次，然后選取得分最高的位姿，返回最終的得分
得到每個粒子的分?jǐn)?shù)之后就是要判斷粒子的得分是否滿足要求，接下來是（2）現(xiàn)在我們返回到scanMatch（）函數(shù)中在optimize函數(shù) 之后就是要根據(jù)當(dāng)前粒子計算粒子的權(quán)重：并處理匹配失敗的情況m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);（3）在scanmatcher()函數(shù)中第三個重要的函數(shù)是根據(jù)粒子的權(quán)重，更新地圖m_matcher.computeActiveArea(it->map, it->pose, plainReading); //用于計算每個粒子相應(yīng)的位姿所掃描到的區(qū)域 ，   //計算過程首先考慮了每個粒子的掃描范圍會不會超過子地圖的大小，如果會，則resize地圖的大小
//然后定義了一個activeArea 用于設(shè)置可活動區(qū)域，調(diào)用了gridLine() 函數(shù),這個函數(shù)如何實現(xiàn)的，我不了解了
每次處理完一次的測量數(shù)據(jù)，之后還要進行權(quán)重更新。
updateTreeWeights(false);//權(quán)重更新
粒子集對目標(biāo)分布的近似越差 權(quán)重的方差越大，在重采樣的過程中還調(diào)用了registerScan ，這個函數(shù)和computeActive 函數(shù)有點像，不同的是，registerScan用于注冊每個單元格的狀態(tài)，自由、障礙，調(diào)用update()以及entroy()函數(shù)更新，最后是障礙物的概率 p=n/visits,障礙物的坐標(biāo)用平均值來算完了后，又有一次權(quán)重計算。之后就是要重采樣
到此處processScan 結(jié)束，回到laserCallback，還有最優(yōu)一步updateMap
返回到laserCallback（）函數(shù)之后就是要執(zhí)行以下函數(shù)updateMap(*scan);   //地圖更新，先得到最優(yōu)的粒子（用權(quán)重和 weightSum判斷 ），得到機器人最優(yōu)軌跡 

總結(jié)

以上是生活随笔為你收集整理的Gmapping 乱七八糟的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。