基于經典RTAB-MAP框架的三維重建系統(tǒng)分析目錄TOC\o"1-3"\h\u27422基于經典RTAB-MAP框架的三維重建系統(tǒng)分析 1285311.1RTAB-MAP算法介紹 1281261.1.1圖slam 1154371.1.2傳統(tǒng)RTAB-MAP算法框架 2286991.2關鍵算法描述 4171021.1.1坐標系轉換 5240821.1.2ICP算法 6310061.1.3TSDF點云融合算法 7114231.1.4g2o后端優(yōu)化算法 81.1RTAB-MAP算法介紹RTAB-MAP是一個目前廣泛流行的開源SLAM算法，它是一種基于環(huán)境的閉環(huán)檢測方法。其具有內存管理功能來處理大規(guī)模和長期的在線計算。之后，它逐漸發(fā)展為在各種機器人和移動平臺上實行SLAM算法。由于每個應用程序在傳感器，處理能力和運算能力方面都有自己的優(yōu)缺點，因此，如何根據所用的傳感器選擇合適的算法極為重要。RTAB-MAP算法涵蓋了大部分傳感器及復雜環(huán)境的應用。用戶可以使用不同的機器人和傳感器來運行并比較各種方案來選擇最合適自己的算法工具。接下來主要介紹RTAB-MAP的背景知識及算法分析。1.1.1圖slam圖SLAM(GraphSLAM)是一種SLAM算法，可以解決完整的SLAM問題，該算法可以恢復整個地圖和路徑而不是僅僅是最近的位姿和地圖。圖SLAM的優(yōu)點之一是減少了對車載處理器計算能力的需求，另一個是圖SLAM的圖形比FastSLAM的地圖精度更高?？焖貴astSLAM使用粒子來估計機器人最可能的位姿，但是在任何時間點，都有可能在最可能的位置沒有粒子。由于圖SLAM解決了完整的SLAM問題，因此這意味著它可以一次處理所有數據以找到最佳解決方案。在圖SLAM中，其思想是在圖中組織信息。圖中的節(jié)點表示在特定時間步長t上的機器人姿態(tài)或環(huán)境中的特征的位置。圖中的邊代表姿勢和特征之間的測量約束或兩個連續(xù)姿勢之間的運動約束。由于空間約束較弱，可以將它們視為連接兩個帶有質量的彈簧。以此類推，完整的SLAM問題可以作為全局圖優(yōu)化問題來解決。最佳的配置是使彈簧松弛并且使每個節(jié)點上的力最小的結構。最大似然原理（MLE）用于優(yōu)化地圖。當應用于SLAM時，似然度會嘗試根據給定的運動和測量觀測值來估計狀態(tài)和特征位置的最可能配置。圖SLAM的目的是創(chuàng)建一個環(huán)境中遇到的所有機器人姿勢和特征以及最可能的機器人路徑和環(huán)境圖的圖。該任務可以分為兩部分，前端和后端。圖SLAM的前端著眼于如何使用機器人收集的里程計和測量值來構建圖。這包括解釋傳感器數據，創(chuàng)建圖形并在機器人穿越環(huán)境時繼續(xù)向其添加節(jié)點和邊。當然，這取決于期望的目標，包括應用的精度，所使用的傳感器和其他因素，例如，前端可能因應用場景而異。在辦公室中使用激光測距儀應用SLAM的移動機器人的前端與在大型室外環(huán)境中使用立體攝像機的車輛的前端有很大不同。圖SLAM的前端也面臨著解決數據關聯(lián)問題的挑戰(zhàn)。用簡單的術語來說，這意味著可以準確地識別以前是否已經看到過環(huán)境中的特征。圖SLAM的后端的輸入是帶有所有約束的完整圖形，輸出是機器人姿勢和地圖特征最可能的配置。后端是一個優(yōu)化過程，它接受所有約束并找到產生最小錯誤的系統(tǒng)配置。后端在各個應用程序之間更加協(xié)調。前端和后端可以連續(xù)完成，也可以迭代執(zhí)行，后端將更新后的圖形饋送到前端以進行進一步處理。本文所利用的RTAB-MAP便是基于圖的SLAM?；趫D的SLAM意味著該算法使用從視覺傳感器收集的數據來定位機器人并繪制環(huán)境圖。在基于圖的方法中，稱為“回環(huán)檢測”的過程用于確定機器人之前是否曾看到過某個位置。隨著機器人前往其環(huán)境中的新區(qū)域，地圖會擴展，每個新圖像必須與之進行比較的圖像數量也會增加。這導致回環(huán)檢測會花費更長的時間，并且復雜度線性增加。RTAB-MAP通過使用多種策略來實時關閉環(huán)路，針對大規(guī)模和長期SLAM進行了優(yōu)化。1.1.2傳統(tǒng)RTAB-MAP算法框架圖STYLEREF1\s2SEQ圖表\*ARABIC\s11RTAB-MAP框架圖圖STYLEREF1\s21所示，RTAB-MAPROS節(jié)點所需的輸入是：任何來源輸入的里程計（可以是3個自由度或6個自由度）；視覺數據輸入（來自攝像頭獲取的RGB-D圖像或立體圖像）以及相應的校準消息，可選輸入是來自2D激光雷達的激光掃描或來3D激光雷達的點云。然后，將這些輸入的所有消息同步并傳遞到圖-SLAM算法。輸出為：地圖數據；沒有任何數據的地圖；里程計校正發(fā)布在TF上；可選的OctoMap（3D占用網格）；可選的密集點云；可選的2D占用柵格。圖STYLEREF1\s21為RTAB-MAP的主要ROS節(jié)點。由圖可知里程計只是RTAB-MAP的外部輸入，RTAB-MAP幾乎可以兼容任意一種里程計設備。各種傳感器數據同步后，短期記憶模塊將創(chuàng)建一個節(jié)點，用于存儲里程計姿態(tài)、傳感器的原始數據以及對下一個模塊有用的其他信息。根據從節(jié)點創(chuàng)建的數據相互重疊的多少，以毫秒為單位設置固定速率來創(chuàng)建節(jié)點。例如，如果機器人運動迅速且傳感器范圍較小，則應提高檢測率以確保連續(xù)節(jié)點的數據重疊，但是將其設置得過高則會不必要地增加內存使用量和計算時間。兩個節(jié)點之間的剛性轉換關系存在鏈接中，一共有三種鏈接：鄰居鏈接，閉環(huán)鏈接和鄰近鏈接。通過里程計轉換，在相鄰節(jié)點之間的短期內存中添加鄰居鏈接?；丨h(huán)和相鄰鏈接分別通過回環(huán)檢測和鄰近檢測來添加。所有的鏈接都用作圖優(yōu)化的約束。當向圖添加新的閉環(huán)或鄰近鏈接時，圖優(yōu)化會將計算出的誤差傳播到整個圖，以減少里程計的漂移。通過圖優(yōu)化，占據地圖，點云和2D柵格能夠組合輸出，并將其發(fā)布到外部模塊。此外，還可以利用測距法在地圖框架中進行定位。RTAB-MAP的內存管理方法在圖形管理模塊上運行[20]。它用于限制圖形的大小，以便可以在大型環(huán)境中進行在線SLAM計算。隨著圖形的不斷增長，在閉環(huán)檢測、圖優(yōu)化和全局地圖組裝等模塊的處理時間會變長，為了避免這段處理時間大于節(jié)點獲取的周期時間，RTAB-MAP引入了內存管理的理念RTAB-MAP的內存分為工作內存（WM）和長期內存（LTM）兩種。當節(jié)點數過多導致定位匹配的時間超出固定時間閥值“RTAB-MAP/TimeThr”時，RTAB-MAP就將工作內存中不太可能形成閉環(huán)的節(jié)點轉移到長期內存中，而這些節(jié)點就不參與下次閉環(huán)檢測的運算了，這樣可以有效避免由于工作內存過大導致更新時間過長。除此之外，還有一個內存閾值“RTAB-MAP/MemoryThr”用于設置工作內存可以容納的最大節(jié)點數。要確定哪一個節(jié)點轉移到長期內存，需使用啟發(fā)式方法（例如，觀察到的位置越長，則越重要）就顯得尤為重要，因此應將其留在工作內存中。為此，在創(chuàng)建新節(jié)點時，短期內存將該節(jié)點的權重初始化為0，然后將其可視化地進行比較（得出相應的可視化單詞的百分比）與圖形中的最后一個節(jié)點進行比較。RTAB-MAP設置了一個相似閾值“Mem/RehearsalSimilarity”，當新節(jié)點與圖形中最后一個節(jié)點的相似度超出這個閾值，則將這兩個節(jié)點的權重相加賦值給新節(jié)點，圖形中最后一個節(jié)點的權重將重置為0。如果機器人沒有移動，則最后一個節(jié)點將被丟棄，以避免無用地增加圖形大小。當達到時間或內存閾值時，最小的加權節(jié)點中最早的節(jié)點將首先傳輸到長期內存。當工作內存中某一節(jié)點檢測到閉合環(huán)路時，可以將該節(jié)點的鄰居節(jié)點從長期內存重新帶回到工作內存中，以進行更多的回環(huán)檢測和鄰近檢測。當機器人在先前訪問的區(qū)域中移動時，它可以逐漸記住過去的位置，以擴展當前的組合圖并使用過去的位置進行定位。RTAB-MAP可用于對不同的傳感器進行試驗，并盡早確定傳感器是否適合目標應用。根據論文提供的結果，可以導出有關在室內環(huán)境中使用SLAM（無外部全局定位）的準則。除非使用遠程激光雷達，否則對于魯棒的自主導航，必須具有來自本體感受傳感器（例如慣性導航，車輪編碼器）的里程輸入。當僅依靠短距離傳感器時，機器人很可能會終止于傳感器無法看到足夠特征以能夠在地圖上定位自己的區(qū)域。對于相機，看到白色的墻壁，無紋理的區(qū)域或黑暗的區(qū)域會導致定位不準確。對于短距離激光雷達，較大的空白空間或幾何形狀復雜度較低的長走廊也可能會產生問題。兩種傳感器的問題都取決于環(huán)境。1.2關鍵算法描述基于Kinect2的室內三維重建在經典架構上主要分為前端VO部分，中間的關鍵幀提取，角點檢測，ICP匹配，后端優(yōu)化與回環(huán)檢測等幾個部分。下文將對本文用到的一些算法進行數學模型的分析，方便對算法進行進一步的介紹。首先介紹在重建過程中需要用到的幾個坐標，接著闡述各個坐標系之間的轉換關系，以及相機位姿的變換矩陣，這些轉換關系是聯(lián)系相機與最終的重建結果的基礎。然后我將介紹點云配準的方法ICP算法與TSDF點云融合的算法，這部分用于完成從一幀的點云到整個的點云的過程。最后介紹以下g2o的后端優(yōu)化框架用來進一步融合UWB數據提高最后建圖的精度。1.1.1坐標系轉換在整個重建系統(tǒng)中包含著三種坐標系，全局的世界坐標系（機器人的運動軌跡等都是在世界坐標系中觀測的）、相機坐標系以及圖像坐標系，通過相機的內參矩陣可以將圖像坐標系轉換到相機的坐標系上，然后通過剛體變化將相機坐標系再轉換到最后的世界坐標系上。（2-1）上述公式描述了圖像坐標與像素坐標的關系，其中（u，v）為像素坐標系，（x，y）為圖像坐標系。（2-2）上述公式描述了相機坐標系與圖像坐標系的轉化關系。其中（XC，YC，ZC）坐標為相機坐標系，（x，y）為圖像坐標系.，相機的光軸為XZ，XCYC與圖像坐標系的xy軸平行，為光軸與相機物理坐標系的夾角，f為相機的焦距。（2-3）上述公式描述了世界坐標系（XWYWZW）與圖像像素坐標系（u，v）的關系即通過外參矩陣得到相機坐標系與世界坐標的關系然后再通過公式（2-2）以及（2-1）得到在假設相機光軸與圖像物理坐標系夾角為90度的情況下得到的。相機運動可以用歐拉角和四元數進行描述，其中歐拉角通過roll，pitch，yaw三個角度來描述物體在不同坐標軸上的旋轉分量來描述物體整體的旋轉狀況，但是容易出現(xiàn)萬向節(jié)死鎖問題。旋轉過程中的坐標軸重合會使得得到的旋轉狀態(tài)失去在某些坐標軸上的分量。四元數則使用了冗余的九個相關量來描述旋轉過程，使得整個旋轉過程的描述不會出現(xiàn)歐拉角的萬向節(jié)死鎖問題具有奇異與冗余的性質。四元數q的表達式為一個實部加上3個虛部，如下表達式：（2-4）描述旋轉矩陣可以由四元數的參數表示如下式所示（2-5）1.1.2ICP算法ICP（IterativeClosestpoint）算法由Besl和Mckay提出。ICP算法通過兩個點集之間距離最近為原則建立誤差函數。然后通過使得優(yōu)化函數取最小值來獲取變換矩陣使得點云能夠進行精確匹配。ICP算法在執(zhí)行過程中主要分為以下四步：1）對于匹配點進行計算，找到源點集對應的目標點集。2）通過源點集與目標點集估計變換矩陣并求目標函數最小值。3）對源點集進行轉換得到新的點集作為新的源點集4）對上述過程進行迭代計算直到目標函數值變化小于閾值。目標函數由下式給出（2-6）其中d是目標函數N為點的個數W為匹配點集U為當前的源點集T為目標點集R為轉換矩陣。在實際使用Kinect2進行slam重建的時候不可能使用整張深度圖上的所有點云數據進行ICP算法匹配。所以需要選取一些關鍵點進行幀間匹配。首先通過圖像金字塔對圖片進行降采樣的處理得到分辨率較小的圖片，然后通過UWB對于相機姿態(tài)的估計來判斷關鍵幀之間可能匹配的點的區(qū)域然后再在這些區(qū)域選取關鍵點進行ICP算法匹配以降低運算成本。1.1.3TSDF點云融合算法點云通過ICP算法完成匹配后通過TSDF(TruncatedSignedDistanceFunction)算法來進行全局的重建，這個算法能夠使得重建出來的目標的細節(jié)得到更多的保留。TSDF算法需要先創(chuàng)建一個全局立方體用來儲存接受到點云中的點。按所需要重建的范圍大小來劃分全局立方體成對應的小立方體然后按早以下步驟來完成對于一幀點云的融合。首先通過ICP算法拿到變化矩陣并且將立方體中間的方塊通過坐標系轉換的方法得到其相機坐標。通過標定得到的相機內參矩陣。將1）得到的相機坐標轉換到圖像坐標。獲取該點的深度值與相機坐標中的值進行比較判斷該立方體在重建的表面的位置。在內部還是外部。4）更新立方體的權重與距離權重與距離的計算公式如下(2-7)上面第一個式子用來計算相鄰兩幀之間的小立方體的重建時候的表面距離。第二個式子則是計算其權重。第三個式子中的maxtruncation和mintruncation則表示其截斷閾值。1.1.4g2o后端優(yōu)化算法SLAM中間很重要的一部分就是對于機器人位姿的估計，這一過程在本質上也是一個對于非線性誤差函數求解最小值的問題，在數學上這一問題有很多通用方法比如高斯-牛頓法，LM法等，但是這些方法在用于工程上的時候相當的困難。g2o則是一個圖優(yōu)化的開源