博客轉載自：https://blog.csdn.net/zhubaohua_bupt/article/details/77093334
繪制二維障礙柵格地圖-局部地圖的繪制 ：http://blog.csdn.net/zhubaohua_bupt/article/details/72923373 講述了如何利用深度相機去繪制單幀圖像看到的局部地圖。但是只有局部地圖是不夠用的，在一般的地面機器人的路徑規划中，往往需要整個場景的柵格地圖，這樣，就可以為機器人，在場景里所有可達的目標位置，提供路徑規划數據。本篇博客是接上述那篇博客講述的。本篇博客內容為：在已知局部柵格地圖后，怎么通過相機在不同時刻的位姿，把多幀局部地圖繪制成全局柵格地圖。比如，這是一個局部地圖：
不斷融合后，形成的全局地圖：
關鍵技術點： 1: 坐標轉換: 局部柵格地圖根據相機位姿，把二維柵格點轉換到不斷融合的全局地圖 2. 地圖融合: 不斷融合的全局柵格地圖，和新的一幀局部柵格地圖的融合
坐標轉換
坐標轉換是把t時刻的局部地圖點，經過相機t時刻的位姿，投射到全局地圖中。柵格地圖的應用對象是地面機器人，它的運動一般是二維的，所以，在坐標轉換過程中，我們只需要考慮相機當前坐標系與初始坐標系（一般用它來作為全局參考系）的偏航角Yaw與t時刻相機中心在全局中心的位置。以下默認坐標系(Z軸：相機前方向，Y軸：相機下方向，X軸：相機左方向)
偏航角的計算：如果我們知道了t時刻相機相對於全局參考坐標系的位姿T(R，t)之后，只需要姿態R，就可以轉換得到偏航角Yaw（不明白就搜旋轉矩陣轉RPY角）
公式如下
其中， R為t時刻相機相對於參考系的旋轉矩陣，R31為旋轉矩陣第三行第一列的元素，其他類推。
t時刻相機中心在全局中心的位置:
地圖融合
這一部分講述的是，把局部地圖投影到全局地圖后，怎么和已有的全局地圖進行融合，形成一個新的全局地圖。我們知道，柵格地圖至少有三種狀態（可通行（無障礙占據）、有障礙占據、未知），融合是解決這樣一個具體問題：在全局柵格地圖中，某一個柵格（稱為g）肯定是這些狀態中的一種，當局部地圖上的點伴隨着它自己的狀態，投影到柵格g上時，怎么解決柵格g狀態更新的問題。常用的融合策略是概率融合（搜概率柵格圖）。在《局部地圖》這篇博客中，我們畫的局部地圖其實就是概率柵.格圖，初始時，用像素值128表示當前柵格不確定（所用像素值的范圍0-255），然后，如果當前柵格是障礙，我們把它置0，否則就是255。
融合方法1: 概率統計
全局柵格地圖上柵格的值，我們不讓它是3值的（3狀態），而是讓它們分布在0-255，柵格對應的值小於128時，越小是障礙的概率越大，柵格對應的值大於128時，越大是障礙的概率越小。如此，我們就可利用如下思想融合地圖：
如此，我們就可這樣融合地圖： IF plocalGrid==obstacle and plocalGrid -value1>=0 Then pglobleGrid-=value1; ELSE IF plocalGrid== PassableandpglobleGrid +value2<=255 Then pglobleGrid +=value2; Endif
value1和value2是一個經驗值，比如可以都取5。注:pglobleGrid為局部柵格plocalGrid投影到全局柵格上后，在全局柵格上的位置。柵格地圖的像素值在0~255之間。這個方法雖然比較暴力，但也是基於統計的，效果可行。
融合方法2: 貝葉斯濾波
更普通的方法是利用貝葉斯濾波對柵格地圖融合。先給出公式
這個公式乍一看有點復雜，但稍微經過解釋，非常容易理解。下面我們來看看，這個公式表述了一個怎樣的融合方法。首先解釋一下公式中的符號含義：
z : 二值化觀測值（每一次的觀測值來源於局部地圖，只有兩種狀態：占據、空閑）
融合t+1時刻的觀測值后，柵格i被障礙占據的概率。
融合t時刻的觀測值后，柵格i被障礙占據的概率。
單純的從t+1時刻的觀測值，判斷的柵格i被障礙占據的概率
一般取常數，比如
注意：這里
不是一個完備事件，也就是說兩者之和不一定為1。說到這里，講的可能還不是特別清楚，我們舉個融合的例子來幫助理解。
通過這兩個例子，也就是新觀測不同的時候，全局地圖融合的過程，你是否能理解這個公式呢？
地圖融合注意事項
在融合地圖時，我們只融合當前局部地圖能看到的范圍，我們不能往全局地圖里融合未知的范圍，否則就會出錯。舉個例子，比如你的全局地圖剛才看到了一面牆后面的場景，當前局部地圖看到的是相機至這面牆之間的空間，牆后面的看不到（對於局部地圖是未知區域），這個時候，如果把未知融合到全局地里，肯定不是我們想要的。最后，地圖構建的效果見頂端。