大規(guī)模環(huán)境下基于圖優(yōu)化SLAM的后端優(yōu)化方法

王忠立，趙 杰，蔡鶴皋

(1.北京交通大學電子信息工程學院，100044北京;2.機器人技術與系統(tǒng)國家重點實驗室(哈爾濱工業(yè)大學)，150080哈爾濱)

隨著移動機器人的應用逐步從小規(guī)模、靜態(tài)環(huán)境向大規(guī)模環(huán)境發(fā)展，基于圖優(yōu)化的SLAM方法成為研究熱點.從實現(xiàn)框架上，可以將SLAM分解為前端的圖構建和后端的圖優(yōu)化兩個過程.相對于早期的基于遞歸貝葉斯狀態(tài)估計理論方法，如基于Kalman濾波和基于粒子濾波(PF)的方法，圖優(yōu)化SLAM方法是先通過前端的圖構建過程得到初始狀態(tài)估計，然后在后端對初始狀態(tài)估計進一步優(yōu)化求解，因此地圖的一致性和精度更好.基于圖優(yōu)化的SLAM方法是一種batch方法.在文獻［1］中，詳細介紹了圖優(yōu)化建模方法及前端的數(shù)據(jù)關聯(lián)和環(huán)形閉合檢測方法.本文主要對基于圖優(yōu)化SLAM的后端優(yōu)化方法進行總結.

在前端圖構建得到的“初始圖”中，由于傳感器的噪聲、系統(tǒng)參數(shù)的不確定性、環(huán)形閉合檢測的誤差乃至錯誤等因素，使得圖優(yōu)化過程具有很大的挑戰(zhàn)性.不僅要求優(yōu)化方法具有較好的魯棒性、穩(wěn)定性，同時，優(yōu)化過程通常是對整個地圖進行的，在大規(guī)模環(huán)境下，也要考慮優(yōu)化方法的計算復雜度問題.國內(nèi)外學者提出了各種優(yōu)化計算方法，主要包括:基于最小二乘法的優(yōu)化方法，基于松弛迭代的優(yōu)化方法，基于隨機梯度下降的優(yōu)化方法，以及基于流形的優(yōu)化方法等.并對基于圖優(yōu)化SLAM的計算效率、魯棒性和可擴展性等方面展開了研究.但對算法的性能及地圖重建結果評估的研究相對較少.針對大規(guī)模環(huán)境下的地圖創(chuàng)建，地圖創(chuàng)建質量的評估也是非常重要的一環(huán).為此，本文對兩種地圖質量評價方法進行了總結，對優(yōu)化方法存在的挑戰(zhàn)性進行了闡述，在文獻［2］基礎上，介紹了最新的研究進展，并對發(fā)展趨勢進行了展望.

1 基于圖優(yōu)化SLAM的后端優(yōu)化方法

1.1 優(yōu)化方法概述

基于圖優(yōu)化的SLAM方法，是利用圖模型對SLAM問題進行建模.模型中的節(jié)點對應不同時刻機器人及環(huán)境組成系統(tǒng)的狀態(tài)，邊則描述了系統(tǒng)狀態(tài)(節(jié)點)之間的約束關系［3］.這類方法將SLAM問題劃分為前端(front-end)和后端(backend)兩個部分，如圖1所示.前端完成圖的構建，即根據(jù)觀測和系統(tǒng)約束構建圖的節(jié)點和邊;后端主要完成圖的優(yōu)化.基于圖優(yōu)化的方法利用所有的觀測信息來優(yōu)化估計機器人完整的運動軌跡，因此也稱為全SLAM方法.

圖1 基于圖優(yōu)化的SLAM框架

在前端部分，順序數(shù)據(jù)關聯(lián)是指相鄰觀測數(shù)據(jù)幀間的匹配及相對姿態(tài)估計問題，而環(huán)形閉合檢測則根據(jù)觀測數(shù)據(jù)判斷機器人是否處在之前已訪問過的環(huán)境中.二者的核心是要解決數(shù)據(jù)關聯(lián)問題，前者考慮局部數(shù)據(jù)關聯(lián)，而后者則涉及全局數(shù)據(jù)關聯(lián).順序數(shù)據(jù)關聯(lián)與環(huán)形閉合檢測都是根據(jù)觀測信息建立圖節(jié)點間的約束，即完成圖的構建，是基于圖優(yōu)化方法前端的兩個核心部分.

由于觀測噪聲以及數(shù)據(jù)關聯(lián)誤差的存在，前端得到的圖往往存在不一致性.若用Ti來表示數(shù)據(jù)幀間的相對變換矩陣，而且T0，T1，…，Tn構成一個閉環(huán)，則理想情況下，應滿足:

式中Ⅰ為單位矩陣.但通過觀測信息關聯(lián)得到的相對變換矩陣通常不滿足該約束.在基于圖的模型描述中，機器人的位姿是待估計的隨機變量，位姿間的約束則是與隨機變量相關的觀測，圖優(yōu)化結果對應于位姿的最大似然估計.與順序數(shù)據(jù)關聯(lián)及環(huán)形閉合檢測不同，圖優(yōu)化部分一般不直接處理觀測數(shù)據(jù)，而是在前端構造的圖基礎上進行優(yōu)化運算.

Golfarelli等［4］將圖優(yōu)化視為質量-彈簧模型，從另一個視角來解釋圖的優(yōu)化問題，如圖2所示.在該模型中，將機器人的位姿看作是帶質量的節(jié)點(黑色圓點)，而約束則看作是連接這些節(jié)點的彈簧.由于每個約束都是根據(jù)與之相關的觀測獨立求解的，它們之間存在不一致性，此時彈簧處在受力形變狀態(tài)，這樣的物理系統(tǒng)通常并不穩(wěn)定.當彈簧對質點的作用力使系統(tǒng)重新達到平衡時，系統(tǒng)處在能量最小狀態(tài)，此時質點的分布即代表最優(yōu)的位姿序列.彈簧的系數(shù)通過觀測的不確定性來(協(xié)方差)表示.觀測的不確定性越小，彈簧的強度越大，使其形變需要的外部作用力也越大;反之，觀測的不確定性越大，彈簧的強度越小，使其形變需要的外部作用力也就越小.系統(tǒng)的能量最小狀態(tài)對應于非線性最小二乘問題的最優(yōu)解.

圖2 基于圖優(yōu)化的mass-spring模型

基于圖優(yōu)化SLAM的后端優(yōu)化方法，概括起來可以分為基于最小二乘法的優(yōu)化方法，基于松弛迭代的優(yōu)化方法，基于隨機梯度下降的優(yōu)化方法，以及基于流形的優(yōu)化方法等.

1.2 基于最小二乘法的優(yōu)化方法

SLAM可以看作是一個非線性最小二乘問題［5］.基于最小二乘的優(yōu)化方法是通過對目標函數(shù)的一階泰勒展開對系統(tǒng)進行線性化，采用迭代法求解線性系統(tǒng)的解，如 Gauss-Newton方法、Levenberg-Marquardt方法等.如果不考慮SLAM問題的稀疏結構特性并假定圖中的節(jié)點數(shù)為n，則Levenberg-Marquardt算法的時間復雜度為O(n3)［6］，在實際問題求解中遠不能滿足實時性要求［7］.Dellaert和 Kaess［5］提出利用 SLAM 問題中內(nèi)在的稀疏結構特性，通過稀疏矩陣分解(如稀疏Cholesky分解等)來求解的非線性最小二乘的方法.Kaess等［8］將圖建模和稀疏線性代數(shù)方法相結合，提出了iSAM方法，iSAM是增量式圖優(yōu)化方法的代表.該方法通過對平滑信息矩陣作QR分解，并選擇性對其進行增量式更新，從而避免了每次重新計算平滑信息矩陣，提高了更新的效率，處理問題的規(guī)模也大大增加，其最佳復雜度為O(n).在iSAM的基礎上，通過節(jié)點的重新排序和重新線性化，Kaess等［9］又提出了改進后的iSAM2方法.為了進一步提高增量式更新過程的計算效率，Kaess等［10］在 iSAM2的框架下，提出采用貝葉斯樹結構來描述SLAM的方法.在SLAM稀疏特性的研究方法上，Konolige等［11］提出一種根據(jù)給定圖約束快速構造稀疏矩陣的SPA(sparse pose adjustment)方法.

1.3 基于隨機梯度下降法的優(yōu)化方法

Olson等［7］將隨機梯度下降方法應用到位姿圖SLAM的優(yōu)化中.每次迭代時隨機選取圖中的一條邊作為當前約束并計算相應的梯度下降方向，然后在該方向上對目標函數(shù)尋優(yōu).隨機梯度下降方法具有不易陷入局部極值的優(yōu)點，對初始值具有較高的魯棒性.實驗證明，即使初始值與最優(yōu)值相差較遠，甚至在全零初始值或隨機初始值時，隨機梯度下降方法也能取得較好的收斂結果.Grisetti等［12］對Olson等所提的方法進行了改進和拓展，采用樹型結構來描述位姿間的關系并通過增量方式表示待求解的狀態(tài)，從而能更有效地對位姿進行更新.Grisetti等［13］還將樹型結構表示以及隨機梯度方法應用到6自由度位姿的優(yōu)化中，提出了基于樹的網(wǎng)絡優(yōu)化(TORO)方法.

1.4 基于松弛的優(yōu)化方法

Duckett等［14］提出采用 Gauss-Seidel松弛方法實現(xiàn)后端優(yōu)化.其基本思想是依次選取每個節(jié)點，根據(jù)其相鄰節(jié)點的位置及它們之間的約束關系重新計算并更新該節(jié)點的位置，且每次迭代都遍歷所有節(jié)點.在假定方位角已知(如通過電子羅盤測量)的情況下，Duckett等證明了其必收斂于最優(yōu)解.該方法可用于增量式的SLAM中，在每次有新的觀測到來時直接在原來結果基礎上進行更新.該方法的缺陷是，當某條邊的誤差較大時，需要多次迭代才能將誤差分配到其他邊中，而這正是出現(xiàn)環(huán)形閉合時所需要應對的情況.Frese等［15］提出多層次松弛的優(yōu)化策略，并利用多重網(wǎng)格方法求解偏微分方程，從而大大地提高了出現(xiàn)環(huán)形閉合時節(jié)點的優(yōu)化更新效率.

1.5 流形優(yōu)化

以上三類優(yōu)化方法均假定優(yōu)化過程是在歐氏空間中進行的.在歐式空間中，機器人位姿中的旋轉分量的估計可能會出現(xiàn)奇異.為了避免奇異值問題，旋轉分量部分可以采用四元數(shù)法表示，但又產(chǎn)生了額外的自由度，引入不必要的誤差.為此，Grisetti等［16］提出在流形空間中進行優(yōu)化的思想，避免狀態(tài)空間參數(shù)化時可能出現(xiàn)的奇異值問題，提出了一種分層優(yōu)化的圖優(yōu)化技術(即HOGMAN方法).該方法在在線建圖過程中，根據(jù)當前的觀測約束，對地圖的修正只在上層(粗略描述層)進行，從而提高了效率.最近，研究者們提供了能用于流形優(yōu)化的開源工具(g2o).Kummerle等［17］將HOG-MAN方法和Konolige等人提出的SPA思想結合起來，提出了基于流形的圖優(yōu)化通用框架(g2o框架)，大大提高了開發(fā)效率.在g2o框架的基礎上，Kummerle等［18］進一步擴展了狀態(tài)空間，如增加描述可能隨時間變化的系統(tǒng)參數(shù)，從而可實現(xiàn)同步傳感器標定、建圖和機器人定位任務的方法.Hertzberg［19］和 Wagner［20］等也將流形方法擴展到傳感器的融合和標定問題中，取得了初步成果.

1.6 圖優(yōu)化的計算效率、魯棒性和擴展性

非線性最小二乘法的一個不足是對初始值的依賴，如果給定的初始值離最優(yōu)解距離遠，則很容易陷入局部極值點.為此，Carlone等［21］提出對基于圖優(yōu)化的SLAM作線性近似并給出解析求解的方法，可以利用其結果作為非線性最小二乘方法的初始值.但目前該方法只適用于2D SLAM.針對非線性最小二乘法對異常點的魯棒性不好的特點，目前已有多種解決方法.Sunderhauf等［22］提出了允許在圖優(yōu)化的過程中改變圖的拓撲結構以剔除錯誤的環(huán)形閉合，從而提高了方法的魯棒性.在隨機梯度下降法(SGD)和Levenberg-Marquardt方法的基礎上，提出了前置濾波SGD和前置濾波Levenberg-Marquardt方法，二者均是在優(yōu)化操作之前，利用一個前置濾波器來完成一些預處理過程，以確保全局一致性.這些方法的應用使得非線性最小二乘法的魯棒性有所提高.

當機器人在大小固定的環(huán)境中行走時，圖的節(jié)點數(shù)目應該跟環(huán)境的規(guī)模大小相關，而不是與機器人運動軌跡的長度相關.因此，要使SLAM方法具備良好的擴展性，關鍵是對圖節(jié)點進行有效的控制.減少節(jié)點數(shù)最為直觀的方法是對節(jié)點間的距離進行限制，即只有節(jié)點間的距離超過一定的閾值時才添加到圖中［23］.Kretzschmar 等［24］從觀測所含的信息出發(fā)，評估觀測幀的信息增益，并依此對圖進行剪枝，以控制節(jié)點數(shù)目.該方法在保持節(jié)點規(guī)模的同時具有最小的信息損失，因而也保證了地圖信息的完整性.對節(jié)點進行剪枝實際對應節(jié)點的邊緣化過程，這可能導致圖的結構變得密集.Kretzschmar等［25］提出采用 Chow-Liu 樹對節(jié)點間的關系作近似描述，以保證節(jié)點連接的稀疏性.Yasir和 Jose等［26］提出利用線段擬合機器人的運動軌跡，從而減小位姿圖規(guī)模的方法.Xiang等［27］提出一種變分辨率的地圖表示方法.

上述研究方法進一步提高了基于圖優(yōu)化方法的計算效率、魯棒性和可擴展性.基于圖優(yōu)化的增量式SLAM方法目前研究最廣泛.

2 地圖質量的評價

由于SLAM問題的復雜性，其結果是多方面綜合，因此要給出一個大而全的評價方法是很困難的.比較合理的方法就是對SLAM的各個子問題分別給出評價的方法.盡管如此，要對一些子問題進行評價也是非常困難的，像視覺領域中對立體視覺算法的評估一樣［28-29］，因為這些子問題本身也很復雜.另外，對于大規(guī)模環(huán)境下的地圖重建結果進行評估時，目前可用于對比分析的數(shù)據(jù)集也很有限.有學者提出了一些通用的數(shù)據(jù)集［30-31］，但用于性能對比時，由于這些數(shù)據(jù)集最初不是用于對比的目的，因此很多沒有真實數(shù)據(jù).這也是目前進行評估時存在的困難之一.

Olson等［32］對地圖優(yōu)化算法的評估方法進行了研究，提出要對全局優(yōu)化(Batch模式，通常是離線的)和在線優(yōu)化分別進行比較.

在假定前端的特征提取、匹配(包括環(huán)形閉合檢測)以及異常點已經(jīng)排除的前提下，地圖優(yōu)化實質是后驗概率估計問題.目前主要有兩種方法:基于χ2誤差和基于均方差(MSE)的評價方法.

2.1 基于χ2誤差和均方差的評價方法比較

研究表明，基于χ2誤差方法的局限性在于:χ2誤差小的地圖優(yōu)化結果不一定比χ2誤差大的更好［33］.如圖3所示，是兩種不同誤差定義方法的優(yōu)化仿真結果比較.圖3(a)是真實值，通過對比圖3(b)和圖3(c)可見，在圖3(b)中的χ2誤差很小，但卻遠遠偏離真實值，而圖3(c)的χ2誤差雖然很大，但卻更接近真實值.

在地圖優(yōu)化問題上，產(chǎn)生如圖3所示結果的主要原因是由于觀測過程具有高度非線性本質，造成優(yōu)化曲面非常復雜.某些情況下，映射問題產(chǎn)生的優(yōu)化曲面有“淺谷”出現(xiàn)，在此處，優(yōu)化表面變化大，但對χ2誤差的影響卻很小.即基于χ2誤差的優(yōu)化目標函數(shù)定義中，機器人位姿估計和地圖特征點位置估計相互耦合的關系在某種程度上抵消了相互誤差對地圖結果的影響.圖3(b)中特征點的位置和機器人的位姿在優(yōu)化過程中同時發(fā)生了偏離，而χ2誤差很小.

由此可見，在大規(guī)模環(huán)境地圖創(chuàng)建中，誤差函數(shù)的定義非常關鍵.常用的基于觀測的誤差定義方法(χ2誤差)有時不一定很有效.基于MSE的誤差定義能得到較好的結果.

2.2 過擬合(over-fitting)問題

在機器學習中，為了得到一致假設而使假設變得過于復雜，在訓練數(shù)據(jù)上能夠獲得比其他假設更好地擬合，但是在訓練數(shù)據(jù)外的數(shù)據(jù)集上，卻不能很好地擬合數(shù)據(jù)，出現(xiàn)了過擬合現(xiàn)象.出現(xiàn)這種現(xiàn)象的主要原因是訓練數(shù)據(jù)中存在噪聲或者訓練數(shù)據(jù)太少.在地圖優(yōu)化估計時，如果平均節(jié)點度(average node degree)小會出現(xiàn)該問題.如圖4所示，機器人沿著一個方格運動，在右下角的區(qū)域，由于節(jié)點度小，存在過擬合問題，導致地圖優(yōu)化結果和實際情況的不一致.解決過擬合問題的方法主要有兩種:提前停止樹的增長或者對已經(jīng)生成的樹按照一定的規(guī)則進行后剪枝.

圖4 過擬合問題

3 發(fā)展趨勢

近年來，SLAM的研究也出現(xiàn)一些新的趨勢:

1)隨著移動機器人的工作環(huán)境從室內(nèi)到室外的擴展，工作空間越來越大，面臨的環(huán)境也越來越復雜.相對于室內(nèi)較為簡單的環(huán)境對象，室外環(huán)境由各種對象組成，有靜止的、移動的，有行人、各種車輛等.如何實現(xiàn)復雜、大規(guī)模、動態(tài)環(huán)境下地圖表示，高精度、高效率的地圖創(chuàng)建，以及移動機器人的自定位是一個很有挑戰(zhàn)性的研究課題，是現(xiàn)階段SLAM問題研究的重點.

2)對終生地圖創(chuàng)建(lifelong mapping)和維護的研究.在傳統(tǒng)意義上，機器人一旦通過SLAM實現(xiàn)了未知環(huán)境的建圖，則建圖任務結束，機器人即可利用已經(jīng)建好的地圖進行定位或運動規(guī)劃.終生地圖研究將機器人長期置于未知環(huán)境中，因此需要應對環(huán)境的變化，并持續(xù)對地圖進行更新、維護.這種對地圖的不斷更新、維護就構成了終生地圖研究的主要內(nèi)容［33］.

3)語義地圖(Semantic map)創(chuàng)建的研究.為了使機器人具備理解場景以及能夠識別場景物體的能力，構造具有語義信息的地圖是一種重要途徑.構建語義地圖，可使機器人能夠更好地為人類提供服務，是SLAM發(fā)展的趨勢之一.

4)基于多傳感器融合的SLAM研究.通過多傳感器信息融合，可以彌補單一傳感器在數(shù)據(jù)獲取時的不足，降低狀態(tài)估計的不確定性，改善數(shù)據(jù)關聯(lián)、環(huán)閉檢測等關鍵環(huán)節(jié)的精度和可靠性，進而提高機器人定位和環(huán)境建圖的精度.多傳感器融合在移動機器人SLAM中的研究和應用將會越來越多.

5)多機器人協(xié)作 SLAM研究.與單機器人SLAM相比，多機器人SLAM問題涉及多幾個機器人得到的子圖如何拼接得到全局地圖，以及在全局地圖中各個機器人的定位問題.多機器人協(xié)作完成SLAM具有更準確、更高效率的優(yōu)勢，因此受到越來越多的關注，正成為一個熱點研究問題.

6)非歐式空間下的建模與狀態(tài)估計方法.針對SLAM問題，在歐式空間中對機器人的位姿進行求解存在奇異值問題.另外，目前大多數(shù)SLAM方法是基于傳感器空間的，但機器人控制，需要將機器人坐標系和傳感器坐標系之間關聯(lián)考慮，如在基于激光掃描的傳感器中，利用里程計獲取的運動信息，是在機器人坐標系下描述，和觀測傳感器(激光掃描)是不同的坐標系，二者的變換關系是假定準確已知的.雖然可以通過標定得到二者變換關系，但結果也有不確定性，不確定性對SLAM結果的影響目前還未知.Grisetti等［16］提出在流形空間中進行優(yōu)化的思想，其實質是將SLAM問題置于一個高維空間中，不僅可以避免狀態(tài)空間參數(shù)化時可能出現(xiàn)的奇異值問題，還可以將傳感器自身參數(shù)的在線估計等問題統(tǒng)一到一個完整的系統(tǒng)框架下.對非歐式空間中的求解方法的討論，將對SLAM的發(fā)展產(chǎn)生深刻的影響.

［1］王忠立，趙杰，蔡鶴皋.大規(guī)模環(huán)境下基于圖優(yōu)化SLAM的圖構建方法［J］.哈爾濱工業(yè)大學學報，2015，47(1):75-85.

［2］梁明杰，閔華清，羅榮華.基于圖優(yōu)化的同時定位與地圖創(chuàng)建綜述［J］.機器人，2013，35(4):500-512.

［3］GRISETTI G，KUMMERLE R，STACHNISS C，et al.A tutorial on graph-based SLAM［J］.IEEE Transaction on Intelligent Transportation Systems Magazine，2010，2(4):31-43.

［4］GOLFARELLI M，MAIO D，RIZZI S.Elastic correction of dead-reckoning errors in map building［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway:IEEE，1998:905-911.

［5］DELLAERT F，KAESS M.Square root SAM:simultaneous localization and mapping via square rootinformation smoothing［J］.International Journal of Robotics Research，2006，25(12):1181-1203.

［6］ FRESE U，LARSSON P，DUCKETT T.A multilevel relaxation algorithm for simultaneous localization and mapping［J］.IEEE Trans on Robotics，2005，21(2):196-207.

［7］ OLSON E，LEONARD J，TELLER S.Fast iterative alignment of pose graphs with poor initial estimates［C］//IEEE International Conference on Robotics and Automation.Piscataway:IEEE，2006:2262-2269.

［8］KAESS M，RANGANATHAN A，DELLAERT F.iSAM:Incremental smoothing and mapping［J］.IEEE Trans on Robotics，2008，24(6):1365-1378.

［9］ KAESS M，JOHANNSSON H，ROBERTS R，et al.iSAM2:incremental smoothing and mapping with fluid relinearization and incremental variable reordering［C］//Intl Conf on Robotics and Automation(ICRA).Shanghai:IEEE，2011:3281-3288.

［10］KAESS M，JOHANNSSON H，ROBERTS R，et al.iSAM2:incremental smoothing and mapping using the bayes tree［J］.InternationalJournalofRobotics Research，2012，31(2):216-235.

［11］KONOLIGEK， GRISETTIG， KUMMERLER.Efficient sparse pose adjustment for 2D mapping［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway:IEEE，2010:22-29.

［12］GRISETTI G，STACHNISS C，GRZONKA S，et al.A tree parameterization for efficiently computing maximum likelihood maps using gradient descent［C］//Robotics:Science and Systems III.Cambridge:MIT Press，2008:65-72.

［13］GRISETTI G，STACHNISS C，GRZONKA S.TORO-tree-based network optimizer［CP/OL］.［2014-03-15］.http://www.openslam.org/toro.html.

［14］DUCKETT T，MARSLAND S，SHAPIRO J.Learning globally consistentmaps by relaxation［C］//IEEE International Conference on Robotics and Automation.Piscataway:IEEE，2000:3841-3846.

［15］FRESE U，LARSSON P，DUCKETT T.A multilevel relaxation algorithm for simultaneous localization and mapping［J］.IEEE Trans on Robotics，2005，21(2):196-207.

［16］GRISETTI G，KUMMERLE R，STACHNISS C，et al.Hierarchical optimization on manifolds for online 2D and 3D mapping［C］//IEEE International Conference on Robotics and Automation.Piscataway:IEEE，2010:273-278.

［17］KUMMERLE R，GRISETTI G，STRASDAT H，et al.g2o:A general framework for graph optimization［C］//IEEE International Conference on Robotics and Automation.Shanghai:IEEE，2011:3607-3613.

［18］KUMMERLE R，GRISETTI G，BURGARD W.Simultaneous calibration， localization， and mapping ［C］//InternationalConference on IntelligentRobots and Systems.Piscataway:IEEE，2011:3716-3721.

［19］HERTZBERG C，WAGNER R，F(xiàn)RESE U，et al.Integrating generic sensorfusion algorithms with sound state representations through encapsulation of manifolds［J］.Information Fusion，2013，14(1):57-77.

［20］WAGNER R，BIRBACH O，F(xiàn)RESE U.Rapid development of manifold-based graph optimization systems for multisensor calibration and SLAM ［C］//IEEE/RSJ InternationalConference on IntelligentRobots and Systems.Piscataway:IEEE，2011:3305-3312.

［21］CARLONE L，ARAGUES R，CASTELLANOS J A，et al.A first-order solution to simultaneous localization and mapping with graphical models［C］//IEEE International Conference on Robotics and Automation.Shanghai:IEEE，2011:1764-1771.

［22］SUNDERHAUF N，PROTZEL P.Towards a robust back-end for pose graph SLAM［C］//IEEE International Conference on Robotics and Automation(ICRA)，Minnesota:IEEE，2012:1254-1261.

［23］KONOLIGE K，AGRAWAL M.Frame SLAM:from bundle adjustment to real-time visual mapping［J］.IEEE Trans on Robotics，2008，24(5):1066-1077.

［24］KRETZSCHMAR H，GRISETTI G，STACHNISS C.Lifelong map learning for graph-based SLAM in static environments［J］.Kunstliche Intelligenz，2010，24(3):199-206.

［25］KRETZSCHMAR H，STACHNISS C，GRISETTI G.Efficient information theoretic graph pruning for graphbased SLAM with laser range finders［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway:IEEE，2011:865-871.

［26］YASIR L，JOSE N.Go straight，turn right:pose graph reduction through trajectory segmentation using line segments［C］//European Conference on Mobile Robots(ECMR).Barcelona:IEEE，2013:144-149.

［27］XIANG J，TAZAKI Y，INAGAKI S.Autonomous variable-resolution map building for mobile robots in unknown environments［J］.Electeical Engineering in Japan，2014，186(4):59-69.

［28］SCHARSTEIN D，SZELISKI R，HIRSCHMOULLER H.Middlebury stereo vision page［DB/OL］.［2014-03-10］.http://vision.middlebury.edu/stereo/.

［29］SCHARSTEIN D，SZELISKI R.A taxonomy and evaluation of dense two-frame stereo correspondence algorithms［C］//IEEE Workshop on Stereo and Multi-Baseline Vision.Hawaii:IEEE，2002:131-140.

［30］HOWARD A，ROY N.The robotics data set repository(Radish)2003［DB/OL］.［2014-03-10］.http://www.lib.uts.edu.au/data-archive/4843/radish-roboticsdata-set-repository.

［31］NEWMAN P，CORKE P.Editorial:data papers-peer reviewed publication of high quality data sets［J］.International Journal of Robotics Research，2009，28(5):587.

［32］OLSON E，KAESS M.Evaluating the performance of map optimization algorithm［EB/OL］.［2014-03-20］.http://april.eecs.umich.edu/pdfs/olson2009rss.pdf.

［33］KRETZSCHMAR H，GRISETTI G，STACHNISS C.Lifelong map learning for graph-based SLAM in static environments［J］.Kunstliche Intelligenz，2010，24(3):199-206.