karto_slam的ros版本，其中採用的稀疏點調整（the spare pose adjustment(spa)）與掃瞄匹配和閉環檢測相關。landmark越多,記憶體需求越大,然而圖優化方式相比其他方法在大環境下製圖優勢更大，因為他僅包含點的圖(robot pose)，求得位姿後再求map。
搜尋區域：
以里程計估計的位置為中心的乙個矩形區域，用以表示最終位置的可能範圍，在匹配時，遍歷搜尋區域，獲取響應值最高的位置。
查詢表：
對於雷射獲得的資料資訊，以一定的角解析度和角偏移值進行投影，獲取查詢表，用以匹配。
running-scans：
實時維護的區域性雷射資料鏈，首末兩幀距離在一定距離範圍內，且滿足一定資料規模，否則需要刪除末端資料幀。維護當前區域性資料鏈
生成submap(因為匹配的方式是scantomap)
生成查詢表：
查詢表存在的意義就是相比於暴力匹配，不要每次都重新計算每個雷射資料資訊，相同角度不同位置的雷射資料資訊只需要被索引一次
由里程計**可以得到當前預估的姿態角，真實的姿態角必定在附近；
以一定的解析度和偏移值對原機械人座標系下（區域性座標系）表示的雷射資訊進行不同角度的對映，獲得乙個包含n個角度的查詢表。
其中：查詢表->submap（匹配）
移動機械人自身的狀態包括(x,y,θ)(x,y,θ)，通過查詢表的方式有效解決了角度的問題，眼下仍需解決位置的問題，採用離散化搜尋區域進行定位，並利用多解析度提高搜尋速度
響應值的計算如下描述：將查詢表以一定的為宜投到submap上，此時的submap已有running-scans生成，並利用高斯進行模糊，假設總共有n個點被查詢表擊中，擊中的每個點得分不同（高斯模糊的作用），累加得分並除以可以達到的最高得分
查詢表->submap（粗匹配->精匹配）
為提高搜尋效率，採用多解析度的方式，即粗匹配時採用較低的解析度搜尋得到候選區域，再對候選區域進行劃分得到精確求解。
計算節點均值
在每次匹配過程中（粗匹配和精匹配），選取幾個擁有最優響應值的位姿狀態，取平均值作為匹配結果。
最優響應值為乙個，設立乙個10^-6作為容忍度，一旦有其他位姿狀態的響應值與最優響應值在容忍度內，即為相等；最終的節點為它們的平均值。
迭代：粗匹配（獲得均值）->（作為初值）精匹配（得到最終均值）
計算節點協方差——位置協方差（粗匹配）
2、新增頂點和邊
新增頂點：即關鍵幀的位姿
添 加 邊： 當前幀與前一幀、running-scans、near-chains建立連線。
新增邊主要經過三個步驟：
1、link to previsous scan
2、link to running scans
runningscan chain：一定數量且距當前一定距離內的雷射資料鏈。
3、link to other near chains
nearchain: 以當前節點開始廣度優先的方式從graph中遍歷相鄰的一定距離範圍內所有節點，依據當前id從sensormanager中分別遞增與遞減尋找一定範圍內的chain， 生成nearlinkscans.
前兩個步驟都好理解，主要是第三個，以當前節點為搜尋中心，以一定範圍（比如說4m）進行廣度搜尋，得到了所有關聯的節點，然後利用節點生成資料鏈（資料鏈需要包含約5個id連續的關鍵幀）（不包括當前結點，否則放棄該資料鏈），用當前節點進行匹配，若響應值達到一定閾值，產生一條邊，這條邊一端是當前節點，還有一端是資料鏈中質心距離當前節點質心最近的節點
3、回環檢測
回環檢測的操作和新增鄰近邊類似，步驟較為繁瑣：
1、依據當前的節點, 從graph中找到與之相鄰的所有節點(一定距離範圍內)
2、採取廣度優先搜尋的方式，將相鄰（next）與相連（adjacentvertices）新增進nearlinkedscans.
3、從sensormanager中取從前到後，依據id序號挑選與當前在一定距離範圍內，且不在nearlinkedscans中的candidatescans, 當數量達到一定size，返回。
4、loopscanmatcher進行scantomap的匹配，當匹配response 和covariance達到一定要求認為閉環檢測到。得到調整的correct pose。
5、add link to loop : 調整邊（全域性閉環）
6、觸發correctpose: spa優化
第一步，首先去除那些和當前節點的時間相鄰的節點，針對那些在搜尋範圍內，且在時間點上並不相鄰的節點產生乙個資料鏈，然後進行匹配，若響應值大於閾值，新增回環並進行全域性優化。
演算法學習 Union Find演算法
union find演算法有它的實際用途。多用於動態連通的應用場景。union find演算法是 給出兩個節點，判斷它們是否連通，如果連通，是不需要給出具體的路徑的 舉兩個例子作為主要表現 1 在網路連線中，當發現沒有連線的兩個節點，可以把他們連線起來，一旦節點都連線起來，又能把多餘的線拆除，這時候...
演算法學習 KM演算法
km演算法 用於求二分圖的最佳完美匹配 即權值最大的完美匹配 如果你也是個剛來學習km演算法的人 大概的用途肯定還是知道的吧 還是直接說重點吧 首先 理解km演算法前 必須有以下3個概念 1.可行頂標 對於乙個賦值二分圖g x,y,e,w x,y 代表二分圖的兩邊頂點標號 e代表邊 w代表邊的權值 ...
演算法 演算法學習01
貪婪 可以理解為最簡單基礎的求解方式，特點是 短視性 從這個特點入手很容易理解每一步取其最優的原理。雖然最終結果不一定是最好的，但是一定是較好的而且是最簡便的。因此在不過分追求最優結果或者對速度的要求高於結果的情況下，貪婪是不錯的選擇。分治可以理解為大事化小小事好搞，與貪婪的每一步的 串聯 不同，分...