分枝定界圖解
網上對分枝定界的解讀很多都是根據這篇必不可少的論文《Real-Time Loop Closure in 2D LIDAR SLAM》來的。
分枝定界是一種深度優先的樹形搜索方法，避免了暴力搜索帶來的計算量龐大等問題，成爲cartographer的重要組成部分。
其很好地被用於激光slam的迴環檢測中。用當前的scan和全局地圖global map進行匹配，以找到此幀scan的位姿。
所以目的要明確——尋找當前幀的位姿。
下面就開始分枝定界的圖解。希望讀者同論文一起解讀。
參考：
論文：Real-Time Loop Closure in 2D LIDAR SLAM
論文翻譯：https://blog.csdn.net/luohuiwu/article/details/88890307
論文解讀：https://blog.csdn.net/weixin_36976685/article/details/84994701
柵格地圖：https://zhuanlan.zhihu.com/p/21738718
1.柵格地圖
首先需要明確，什麼是柵格地圖，或者叫佔據柵格地圖。
https://zhuanlan.zhihu.com/p/21738718
建議讀者仔細閱讀上述鏈接。當然，可以先接着往下看。我這裏總結一下上述鏈接的內容。
我們將地圖劃分成細小的柵格，每一個柵格可以用柵格中心點（grid point）表示。每一個柵格都有兩個狀態：
（1）free 未被佔據
（2）occupied 被佔據
現實中我們知道，一個柵格被佔據或者不被佔據都是可以直接感知到的。比如我們拿肉眼跑過去看，哪個地方被佔據，哪個地方沒被佔據，都是確定的。但是如果我們用傳感器（激光雷達）去感知這個世界，一切彷彿並沒有那麼確定，這是因爲一個東西的存在——測量噪聲（或者說測量誤差）。
因此，我們沒法準確得知道一個柵格的真正狀態，有可能它被佔據了，但是沒測出來；或是它沒被佔據，但傳感器誤認爲它被佔據了。那麼不確定問題用什麼表述呢？——
概率。
現在我們將每一個柵格賦予一個概率P表示它被佔據了。P=0.6，則表示這個柵格有超過一半的概率（0.6）可能被佔據。
但事實上爲了計算方便等原因，我們對概率值取對數，得到一個量Odd來表示這個柵格被佔據的狀態。Odd越大，表示這個柵格越可能已經被佔據。當然，詳細的從P轉換爲Odd的過程請見上面的鏈接。
現在，激光雷達的感知過程就轉變爲不斷更新每個柵格的Odd的過程。每一幀激光數據進來，都會更新一次每個柵格的Odd。這個過程包含着貝葉斯的思想。
上圖可以看出，在激光掃描之前和之後，一些被掃描到的柵格的Odd的更新。
至此，你可能大致明白柵格地圖是什麼東西了。他是一個似然場，每一個柵格都有屬於自己的狀態。
最後，柵格可以用一個柵格點（grid point）代替，有時候可能更便於理解。這一個柵格可以被叫做一個“像素（pixel）”，其邊長一般稱爲r，即爲像素寬。
用論文中的圖表示的話就是：
中間的叉叉表示grid point，柵格點間距離即爲像素寬r
2.迴環檢測
迴環檢測的目的是，通過當前掃描幀scan的數據，與global map全局的地圖進行匹配，得到當前的scan的位姿。（也就是搭載激光雷達的機器人的位姿）
這裏用個簡圖表示，不再多說。（因爲既然想知道分枝定界，相信你迴環檢測的概念肯定已經很清楚了）
黃色代表此幀機器人座標系下的激光數據，藍色代表全局地圖中的激光點。當然，真實情況肯定不只是這幾個點。我們嘗試找到一個位姿T（二維問題中，T爲(x, y, θ)），能夠使得將黃色的點映射到地圖座標系中，能夠儘可能的接近甚至重合於地圖點。比如上圖中，框內的點是經過旋轉關係可以進行對應的。
那麼事實上，我們說過我們將真實的世界劃分爲柵格。而柵格是有概率的，或者說是狀態值odd的。那麼我們就想找到一個位姿T，使得將激光點數據映射到全局地圖中，能夠有足夠大的概率——即地圖座標系中所有激光點所在的所有柵格的Odd之和足夠大，這樣就意味着迴環上了。
迴環上的意思就是——我們通過這一幀scan的數據和全局地圖的匹配，找到了屬於這個scan的位姿。
那也有可能沒回環上，也就是說，找不到能特別好匹配上全局地圖的位姿，那麼則沒有構成迴環。
那麼如何找到這個位姿T呢？
3.暴力搜索
一個很直觀的思想，我們進行暴力搜索。暴力搜索肯定要確定一個搜索範圍，在搜索範圍內遍歷。
之前說過，T是（x,y,θ）。
根據里程計等等方法，你可以大致知道這個時刻scan的位姿大約是多少，只不過有累計誤差，但也偏的不會太多太多。因此我們考慮，在這個有誤差的位姿附近搜索即可。假設這個包含累積誤差的初始“預估位姿”爲ξ0，那麼我們在它的四周佈置一個搜索框。如14m邊長的正方形。（這裏把位姿叫做ξ，不叫做T，是爲了和論文保持一致的叫法）
請注意，這裏的搜索框，是抽象出來的，數學層面的搜索，可以不理解爲真實世界中放置的搜索框。也千萬不要理解爲柵格地圖。
（原諒我畫的不均勻。。。。。初始位姿ξ0應該是在搜索框正中心的，搜索框邊長是14m）
其中的每一個小格的邊長代表了此搜索框的分辨率。可能是5cm呀，50cm呀，都有可能。
接下來就是在這個搜索框中暴力搜索。每一個交點代表一個（x, y），對於每個交點，又有一組θ需要遍歷。因此就是三層for循環：
for x
----for y
------for θ
當然，我們知道計算三角函數是更耗時的。因此爲了一定程度減小計算量，可以
for θ
----for x
------for y
如何知道遍歷的哪個位姿是最符合要求的、我們想要的、迴環的位姿呢？
這就需要一個評分機制。
對於每一個遍歷出來的(x, y, θ)，我們給它打個分。分最高的我們認爲是迴環上的位姿。
那麼如何進行評分呢？
4.評分機制
我們要在所有遍歷的位姿中，找到得分最高的那個。按照論文中的寫法是：
其中W爲搜索範圍，K表示當前的scan有K個激光點，T表示位姿。M表示什麼呢？也許式子晦澀難懂。這裏我們看圖說話。
我們畫出以下柵格地圖。注意，這裏不是畫的搜索框哦。
假設當前scan幀就打出兩個激光點（當然，實際上一幀激光有好上百個點，這裏是爲了簡化計算過程），一個橙色的，一個藍色的，並且我們通過當前遍歷到的(x1, y1, θ1)將激光點映射到全局地圖座標系中（如上圖）
我們知道，地圖是用佔據柵格表述的。每個柵格有自己的狀態，稱爲Odd。
藍色的點落入的柵格，它的Odd爲0.7，橙色點所在柵格Odd爲0.2。之前說過，Odd越大，表示越有可能被佔據。
接下來進行打分： score(當前遍歷的(x1, y1, θ1)) = 0.2 + 0.7 = 0.9
至於公式中的M nearest(), 表示的就是每個激光點的小分（0.2或是0.7）是：用與他最近的地圖點(grid point)的狀態量表示。也就是激光點所在柵格的狀態量。隨你怎麼叫。
下面開始遍歷第二個，(x2, y2, θ2)，用這個位姿將激光點投影到地圖座標系下。
接下來進行打分： score(當前遍歷的(x2, y2, θ2)) = 0.5 + 0.9 = 1.4
爲了以後講解，我將這種打分規則稱爲：平凡打分法
則我們認爲，(x2, y2, θ2)比(x1, y1, θ1)更有可能是正確的姿態。
這樣，一個個(x, y, θ)不斷遍歷，直至遍歷完畢，找到一個得分最高的，就是我們認爲的迴環解。也就是當前scan的正確位姿，是我們需要的最優解。
我們可以看到，暴力求解是有龐大的計算量的。因爲你要把搜索框（search window）中的位姿以分辨率爲步長全部遍歷一遍。
原論文給出了暴力求解的僞代碼，因爲建議本blog和論文一起食用效果最佳，所以看到這裏，僞代碼應該也會很明白了。無非就是三層for循環。
4.分枝定界
因爲要做到實時閉環檢測，計算量如此之大。所以肯定不可取。
因此，2016年的論文《Real-Time Loop Closure in 2D LIDAR SLAM》使用了一種能夠大大減少計算量，並能夠不遺漏最優解的方法——分枝定界。（相對於多分辨率搜索而言的，但是這裏不講多分辨率搜索，因爲他被分枝定界暴打）
分枝定界（或稱分支限界、分枝上界）是什麼意思呢？
以下爲引用--------------------------------------------------------------------------------------------------------
分枝界限法是由三棲學者查理德·卡普（Richard M.Karp）在20世紀60年代發明，成功求解含有65個城市的旅行商問題，創當時的記錄。“分枝界限法”把問題的可行解展開如樹的分枝，再經由各個分枝中尋找最佳解。
其主要思想：把全部可行的解空間不斷分割爲越來越小的子集（稱爲分支），併爲每個子集內的解的值計算一個下界或上界（稱爲定界）。在每次分支後，對凡是界限超出已知可行解值那些子集不再做進一步分支。這樣，解的許多子集（即搜索樹上的許多結點）就可以不予考慮了，從而縮小了搜索範圍。
以上爲引用--------------------------------------------------------------------------------------------------------
分枝定界是一種深度優先的樹搜索算法。通過對樹進行剪枝，可以縮小了搜索範圍，大大減小計算量。但同時又不會遺漏最優解。
請現在就記住，分枝定界思想，就是不斷縮小搜索範圍的過程。
先讓我們認識以下所謂的 “樹” 是什麼樣子：
上圖是一個簡單的高度3(自下而上每層高度分別爲0,1,2,3)的二叉樹結構。自上而下，從根節點（root node）開始，每個節點被細分爲兩個子節點，直至不能再被劃分的時候，也就是最後一層，height=0。在分枝定界問題中，什麼情況下不能再分了呢？——到達了搜索框的最細分辨率則不能再分，也就是在暴力搜索中，那些所有的交點的集合構成了樹的葉子節點。
也就是說，迴環檢測的最優位姿，一定出現在葉子節點。
聽不懂的話，繼續往下看。
這裏我不想囿於論文，只想把東西說清楚。最後我會補充關於論文裏面數學理論的一些解釋。
如何將樹結構應用於搜索最優位姿中呢？它是怎麼體現於分枝定界方法呢？
把之前暴力求解的搜索框拿來瞅瞅：（再次強調，這裏不是柵格地圖了哦）
密密麻麻的搜索框很不合自己那追求簡單的心靈。（實際上是密集恐懼症。。。。）
搜索範圍太大，步長又太小，太密集了。
下面開始分枝定界（請不要忘記，分枝定界就是不斷地縮小搜索範圍的過程）
首先定義一個最佳得分best_score = 0.9 ， 這個是初始化的最佳得分，隨便設了一個。有什麼用呢？以後再說。
將搜索範圍重新劃分，這時候步長選的大一點，分出來粗一些，考慮到論文中的情況，我這樣分。
以左上角爲原點，分成四份。原點即爲樹的根節點，也就是位姿（x0, y0）。這裏暫不考慮角度，因爲每次進行分枝定界之前都要規定一個角度，這個角度下的分枝定界做完以後，再以一定步長更新換角度，重新開始分枝定界，也就是說，每一次分枝定界的過程中，角度θ是不變的。
通過這一次粗分辨率的分割，我們得到了四個子節點（紅色。注意自己也算是自己的子節點）：
這四個子節點分別爲（x0, y0），（x0, y1），（x1, y0），（x1, y1）
位姿也就是（x0, y0, θ），（x0, y1, θ），（x1, y0, θ），（x1, y1, θ）。
接下來，用這四個位姿，將當前幀scan激光點投影到地圖座標系，跟之前一樣，需要根據評分規則進行打分。注意，這我們是給父節點打分。
但這裏的打分規則有所不同。不再是平凡打分法。
這裏的打分規則與之前的有什麼不一樣呢？
注意，這裏我們對每一個激光點，用四個子節點均進行位姿轉換。每一個激光點就會得到四個分數，找到其中最大的，記錄下來。然後對每一個激光點都這麼算，將一羣最大值相加，作爲節點的最終得分。之所以這麼做是爲了保證父節點的得分大於子節點的得分。
不明白沒關係，看圖說話。
ps：注意這裏是柵格地圖哦，不是搜索範圍。還是之前的橙色和藍色兩個激光點。
對於橙色的激光點，我們將其通過四個子節點不同的位姿變換，即（x0, y0, θ），（x0, y1, θ），（x1, y0, θ），（x1, y1, θ），變換到了柵格地圖的不同區域：
分別佔據了0.2\0.7\0.4\0.2的柵格，然後我們取其中最大的——0.7。記錄下來。
同理，對於藍色的激光點，我們將其通過四個子節點不同的位姿變換，即（x0, y0, θ），（x0, y1, θ），（x1, y0, θ），（x1, y1, θ），變換到了柵格地圖的不同區域：
分別佔據了0.7\0.7\0.2\0.9的柵格，然後我們取其中最大的——0.9。記錄下來。
接下來進行打分： score(當前父節點(x, y, θ)) = 0.7 + 0.9 = 1.6
我將這種打分方法稱爲——貪心打分法（因爲每個激光點都要取最大的小分，太貪心了）
這樣，父節點有了自己的分數。
爲什麼要這樣給父節點打分呢？後面再說。
下面父節點有了自己的分數。並且它分出了四個子節點。
下一步，我們再次細分。
藍色爲上一步子節點再次細分出來的子節點，四個紅點也就變成了相應的父節點。通過這些新的藍色子節點，我們可以用貪心打分計算四個紅色父節點的得分。同樣，這個得分也會比藍色子節點的平凡得分要高。
假如四個紅色節點的貪心得分爲：1.6, 0.2, 1.7, 0.5這四個貪心得分。
還記得我們曾經初始化了一個最佳得分best_score = 0.9嗎？好，0.2 和 0.5 這兩個節點的分數小魚best_score，那麼直接將此節點統領的區域全部刪除，體現在樹上，就是進行了剪枝，將這個父節點的子樹全部減掉。
依次類推。直到搜尋至葉子節點，找到葉子節點中的best_score，將具有此best_score的葉子節點的位姿，認爲是迴環最優解。
由於源碼暫時還沒看。這位博主介紹的計算流程爲：
（參考blog，改了幾個字:https://blog.csdn.net/weixin_36976685/article/details/84994701）
以下爲引用--------------------------------------------------------------------------------------------------------
再來看這張圖假設我們需要去計算檢測匹配的點爲如圖所示16個
則我們第一層搜索精度最低，只列舉其中兩個，並優先考慮靠左（優先考慮可能性最高的）。
對其繼續分層，將其精度提高一倍，又可以列舉出兩個，並優先考慮靠左。
這樣直至最底層，計算出該情況下的平凡得分，最左的底層有兩個值A和B，我們求出最大值，並將其視爲best_score
然後我們返回上一層還未來得及展開的C，計算C的貪心得分並讓它與best_score比較，若best_score依舊最大，則不再考慮C，即不對其進行分層討論。
若C的貪心得分比best_score更大，則對其進行分層，計算D和E的值，我們假設D值大於E，則將D與best_score對比
若D最大，則將D視爲best_score，否則繼續返回搜索。
以上爲引用--------------------------------------------------------------------------------------------------------
上面是一種可能性的計算流程。後面的第五部分，我會根據僞代碼介紹計算流程。
那麼你現在肯定迫不及待得想知道：
爲什麼要這樣給父節點打分呢？
這是爲了保證，父節點用貪心打分法的得分永遠大於子節點平凡打分法的得分。從而保證，一旦父節點的貪心得分小於best_score，那麼這個父節點的子樹全部被剪枝，因爲其子樹的葉子節點的平凡得分肯定小於上面每一層中間節點的貪心得分，所以肯定小於best_score。——遞歸的思想
細節解釋：
對於這一幀scan的每一個激光點，我從所有的位姿子節點裏面挑一個，能夠使得變換後這個激光點的小分最大的那個子節點，然後將這些精挑細選的小分相加。這樣評分，肯定比這種情況要大：對於每一個激光點，我用同一個子節點進行位姿變換然後打小分，小分相加。前者是父節點的貪心打分，後者是子節點的平凡得分。因此，這樣計算，父節點的得分，肯定比子節點的得分要高。
論文中的公式爲：（最大值之和 大於 和的最大值）當然，看不懂這個晦澀式子沒有關係，能理解分枝定界足以。
再總結一下剪枝的原則是：如果子樹的父節點的貪心score小於之前已經待定的best_score，那麼直接將這個節點的子樹全部減掉。best_score首次搜索至葉子節點之前，是設定的一個初始值（比如之前設置的0.9）。當第一次搜索到葉子節點之後，若葉子節點的得分高於best_score，best_score會被更新。
以上的解釋，你是否已經明白，“定界”的含義了呢？
5.論文的僞代碼
論文的算法二給出了簡單形式的僞代碼。算法三則給出了優化後的僞代碼。
下面我們通過樹的感覺，真正體會一下這個僞代碼的流程。爲了計算和解釋方便，暫時不像論文中每次分出來四個子節點，這裏我們用二叉樹。
我給每個節點都標上了序號A-----O
A爲根節點，H------O爲葉子節點，其餘爲中間節點。
我們設置一個棧：stack，棧中，得分高的在頂上，得分低的在下面。
- 第一次循環
先初始化一個best_score = 0.5
B C 的貪心得分爲：B(1.2), C(1.1)
將B C放入stack中，將棧初始化。—— stack : (棧底) C(1.1) - B(1.2) (棧頂)
將棧頂節點B取出 —— stack : (棧底) C (棧頂)
判斷：B(1.2) > best_score(0.5) 且 B不是葉子節點 —— 則繼續進行分枝
D E 的貪心得分爲：D(1.0), E(0.8)
將D E入棧，並按照得分排序 —— stack : (棧底) E(0.8) - D(1.0) - C(1.1) (棧頂)
- 第二次循環
將棧頂節點C取出 —— stack : (棧底) E(0.8) - D(1.0) (棧頂)
判斷：C(1.1) > best_score(0.5) 且 C不是葉子節點 —— 則繼續進行分枝
F G 的貪心得分爲：F(0.6), G(0.2)
將F G入棧，並按照得分排序 —— stack : (棧底) G(0.2) - F(0.6) - E(0.8) - D(1.0) (棧頂)
- 第三次循環
將棧頂節點D取出 —— stack : (棧底) G(0.2) - F(0.6) - E(0.8) (棧頂)
判斷：D(1.0) > best_score(0.5) 且 D不是葉子節點 —— 則繼續進行分枝
H I 的貪心得分爲：H(0.7), I(0.2)
將H I入棧，並按照得分排序 —— stack : (棧底) I(0.2) - G(0.2) - F(0.6) - H(0.7) - E(0.8) (棧頂)
- 第四次循環
E出棧，J K入棧（具體略） —— stack : (棧底) I(0.2) - G(0.2) - F(0.6) - J(0.65) - H(0.7) - K(0.75) (棧頂)
- 第五次循環
K 出棧 —— stack : (棧底) I(0.2) - G(0.2) - F(0.6) - J(0.65) - H(0.7) (棧頂)
判斷： K(0.75) > best_score(0.5) 且 K 是葉子節點 —— 則更新best_score = 0.75，最優解match = K的位姿
- 第六次循環
H 出棧 —— stack : (棧底) I(0.2) - G(0.2) - F(0.6) - J(0.65) (棧頂)
判斷： H(0.7) < best_score(0.75) —— 則不進行操作
- 第七次循環
J 出棧 —— stack : (棧底) I(0.2) - G(0.2) - F(0.6) (棧頂)
判斷： J(0.65) < best_score(0.75) —— 則不進行操作
- 第八次循環
F 出棧 —— stack : (棧底) I(0.2) - G(0.2) (棧頂)
判斷： F(0.6) < best_score(0.75)且F不爲葉子節點 —— 則不進行操作
注意到了吧，上面這一步，把F出棧，然後不進行操作
這不就是剪枝嘛～！把中間節點及其子樹
好了。。。。。。接下來不寫了。。。。寫到這裏很累了。花了整整一晚上時間。
至於論文中的理論數學公式，累了。。。。。以後有時間再說。
有一些表達，看完我的blog，看完我那幾篇參考的blog和知乎。。。。
也就都能理解了。。。。。
6.總結
分枝定界拆開
分枝：樹搜索，擴展子節點的策略。
定界：每個節點的貪心得分大於其所有子樹葉子的得分，是其子樹的“上界”
7.cartographer源代碼補充
以後一定是要看的哦。
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate2D>& candidates, const int candidate_depth,
float min_score) const {
if (candidate_depth == 0) {
// Return the best candidate.
return *candidates.begin();
}
Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);//討論分層並計算目標函數值
best_high_resolution_candidate.score = min_score;//更新下best score
for (const Candidate2D& candidate : candidates) { //在分支定界中for循環用來分層
if (candidate.score <= min_score) { //若該值不優，則減枝
break;
}
std::vector<Candidate2D> higher_resolution_candidates;
const int half_width = 1 << (candidate_depth - 1);
for (int x_offset : {0, half_width}) {
if (candidate.x_index_offset + x_offset >
search_parameters.linear_bounds[candidate.scan_index].max_x) {
break;
}
for (int y_offset : {0, half_width}) {
if (candidate.y_index_offset + y_offset >
search_parameters.linear_bounds[candidate.scan_index].max_y) {
break;
}
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.x_index_offset + x_offset,
candidate.y_index_offset + y_offset, search_parameters);
}
}
ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
discrete_scans, search_parameters,
&higher_resolution_candidates);
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
//下面利用遞歸，繼續搜索
BranchAndBound(discrete_scans, search_parameters,
higher_resolution_candidates, candidate_depth - 1,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}