筆記和總結：

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

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。