[SLAM] a bite of SLAM

SLAM的定義及用途：

如它的名字所告訴我們的：即同時定位（Localization）與建圖（Mapping）。應用場景一般多見于機器人導航，場景識别等任務。

SLAM的主要過程：

跟蹤運動中的相機，估算出其在每個時刻的位置和姿态（用一個包含旋轉和平移資訊的變量來表示：矩陣或者向量），并将相機在不同時刻擷取的圖像幀融合重建成完整的三維地圖。

傳統的SLAM可分為視覺前端和優化後端兩大子產品。

視覺前端：

視覺前端主要完成的任務為：利用運動中的相機在不同時刻擷取到的圖像幀，通過特征比對，求解出相鄰幀之間的相機位姿變換，并完成圖像幀之間的融合，重建出地圖。視覺前端是一個局部優化過程，由于進行優化的限制條件僅限于相鄰圖像幀，是以在相機運動的過程中，容易産生誤差漂移。

視覺前端依賴于機器人搭載的傳感器，常用的傳感器有相機（單目相機、雙目相機、TOF相機），IMU（慣性測量單元），雷射雷達等。單純通過視覺傳感器（即相機）來完成的SLAM又稱為視覺SLAM。同時，根據使用的視覺傳感器的不同，視覺SLAM也具有不同的算法複雜度和優缺點。

使用單目相機作為視覺傳感器，優點是成本低，硬體簡單，缺點是算法複雜，對相機的運動具有一定的條件要求，因為其是通過SFM（Structure From Motion）算法完成深度資訊的計算，是以如果相機在運動時僅僅發生了旋轉而沒有平移的話，則無法通過Triangulation（三角測量）來計算目标的深度資訊。同時，單目相機需要通過相機在不同時刻擷取到的圖像幀作為類似雙目資訊的資料來進行深度重建，是以在初始時刻是無法得到深度資訊的。此外，由于SFM算法計算出的深度資訊并不是真實的深度資訊，因為它畢竟不是真正的雙目深度重建。在雙目相機中，在雙目标定的時候已經通過标定闆的資訊完成了從像素尺度到真實尺度之間的映射。而SFM雖然也是通過相似三角形的關系計算出一個深度資訊，但是該深度資訊是一個相對值，并沒有與真實尺度建立起聯系。它的做法是：對于相機的初始平移，視為機關1。後續的平移，都是以這個機關進行換算的。而SFM重建出的深度，也是以這個機關來表示的。至于這個機關1對應的真實尺度，其實我們并不知道，隻是人為地給定一個值而已（因為僅有一個單目視覺傳感器是無法測量到真實距離的）。是以一般而言，對于單目SLAM，如果具有測量要求，一般都會與一個IMU配合使用，以擷取真實尺度。

使用雙目相機作為視覺傳感器，優點是擷取點雲資料時，受物體表面材質的反光性質的影響比使用輔助光源的深度傳感器方案（如TOF，結構光等）要小，同時其測量距離也相較其他方案要更遠，深度圖的分辨率更高。缺點是計算複雜度大（計算圖像特征及完成特征比對），同時對于特征的設計具有高魯棒性的要求。這将導緻雙目視覺SLAM在建立稠密地圖（dense map）和算法實時性上具有先天的劣勢。

使用結構光方案的深度相機作為視覺傳感器，與雙目相機方案大同小異。原理都是利用了雙目視差和三角測量，不同之處在于，結構光方案對圖像的特征進行了編碼，該編碼特征在計算和比對時相比雙目相機方案的手工設計特征（如ORB，SIFT等）魯棒性更好，計算更快，缺點是結構光對物體表面的材質敏感，如果是具有反射、透射性質的表面，或者是漫反射過強的表面，都會出現結構光編碼資訊的丢失，進而無法正确地比對特征，導緻深度資訊的缺失。

使用TOF方案的深度相機作為視覺傳感器，優點是幀率高，無需通過特征計算及比對來擷取深度資訊，是以計算複雜度低，用于SLAM上具有更優的實時性。缺點同結構光方案，對物體表面的材質比較敏感。此外，TOF相機在圖像分辨率和測量距離上，也同結構光方案，不如雙目相機的方案。

在視覺前端中，主要的任務是完成對相機位姿的估計和建圖，這時得到的還是一個局部優化的粗略結果，不可避免地包含噪聲和誤差，後續還需要通過後端優化，利用更多限制條件對其進行微調，以獲得精确的位姿和地圖。

估計相機位姿的常用算法是ICP（iterative closest point），簡而言之就是尋找源點集中的每個點在目标點集中的最近鄰點（用于判斷最近點的距離可以在歐氏空間或特征空間中，可以是點到點的距離，也可以是點到面的距離）作為其對應點，建立起多組對應點。對于每組對應點，都用同一個仿射變換矩陣來描述這組對應點的坐标之間的關系（即旋轉和平移關系），建立起一個超定方程組，求解出該仿射變換矩陣（之是以每組對應點之間都是同一個仿射變換矩陣，是因為這裡描述的運動是一個剛體變換，源點集和目标點集都各自代表一個剛體對象）。計算出該仿射變換矩陣後，将其應用于源點集上，使源點集整體旋轉平移到目标點集上，再計算變換後的各組對應點的坐标的總誤差，如果小于給定門檻值則停止疊代，如果大于給定門檻值，則重複上述過程，直到兩個點集的對齊（alignment）收斂。

在SLAM中的做法是，通過ICP來完成相鄰幀之間的圖像配準（前一幀作為目标點集，目前幀作為源點集），進而得到相鄰幀之間的相機坐标系之間的旋轉平移關系。通常以初始位置的相機坐标系作為全局坐标系，每一幀對應的相機坐标系相對于全局坐标系的旋轉平移用于描述該幀對應的時刻的相機的位姿。由于所有相鄰幀對之間的旋轉平移都可以通過對該相鄰幀對進行配準來得到，是以每一幀與第一幀之間的旋轉平移也就都可以通過這些中間過程的旋轉平移來計算得到，進而得到每一幀的相機位姿。

除了通過ICP算法直接對相鄰幀的點雲進行三維配準，圖像配準還可以通過重投影算法來完成： 本質上是最小化重投影誤差，即先将目前幀的點雲投影到前一幀的相機坐标系中（投影使用的變換矩陣為需要優化的參數），再從前一幀的相機坐标系投影到前一幀的圖像坐标系中；同時，前一幀的點雲也重投影到其圖像坐标系中。通過最小化該圖像坐标系上這兩個點集裡所有對應點對的總誤差，來得到目前幀和前一幀的相機坐标系之間的變換矩陣（也即相對位姿）。相鄰幀的點雲可以使用重投影算法來完成配準，相鄰幀的RGB圖像也可以使用重投影算法來完成配準，前者稱之為幾何位姿估計（geometric pose estimation），後者稱之為光度位姿估計（photometric pose estimation）。

從配準對象不同的角度進行劃分，位姿估計又可以分為frame to frame和frame to model兩種方式。前者僅僅是利用相鄰幀本身的資訊完成圖像配準，而後者則是利用目前幀與目前已重建的全局地圖在前一幀中的投影來進行圖像配準，由于用于配準的對象所包含的資訊更多，是以配準效果相較于前者更優。

在經典的視覺SLAM算法中，特征計算使用的是人工設計的特征子和描述子，而近年來出現的一些視覺SLAM算法，則是利用了深度神經網絡來提取圖像幀的特征，在魯棒性上相較于經典方法更優。

相機的位姿由一個旋轉變換和一個平移變換來描述。由于相機位姿的求解是一個非線性優化問題，通常的一種求解思路是利用目标函數的梯度下降方向去尋找極值點，即通過不斷地調整下降方向和搜尋步長，以疊代的方式去逼近極值點。高斯牛頓法和LM算法（Levenberg-Marquardt method）就是其中的兩種經典優化算法。而又因為這種疊代更新參數的政策要求參數對加法運算是閉合的，是以需要将不符合該性質的變換矩陣從李群形式轉化為符合該性質的李代數形式，也即是使用一個無限制的向量來描述相機的旋轉和平移運動。

在完成了位姿估計後，就可以通過每一幀的全局位姿，不斷地将新的圖像幀中的點雲變換到全局坐标系中，與已重建的全局地圖/模型完成融合（常見的融合方式是對在空間中非常接近的幾個點進行權重融合，與相機的入射光線徑向距離近的，與相機的光心距離近的，配置設定較大權重），逐幀地更新重建地圖。對于稀疏SLAM（sparse SLAM）而言，在視覺前端中的建圖部分至此就結束了。而對于稠密SLAM（dense SLAM）而言，此時得到的地圖不僅是不夠精确的，而且在可視化上式不夠直覺的，就是一群離散的點。是以對于稠密SLAM而言，還多了一個步驟，就是對地圖/模型進行渲染。常用的兩種表征模型是網格模型和面片模型，前者的代表為TSDF（Truncated Signed Distance Function），後者的代表為surfel（SURFace ELement）。

TSDF是将全局三維空間劃分為一個三維網格，每個三維小格子儲存其代表的三維空間位置距離相機光心的距離訓示值。正數代表在物體表面的後面，負數代表在物體表面的前面，零值代表在物體的表面上，絕對值的大小表征距離物體表面的遠近，超過-1和1的部分被截斷為截斷值。基于TSDF的稠密建圖，是不斷地利用位于不同幀的相機從不同視角獲得的資訊，從該視角将該幀的資訊重投影到TSDF網格上，進而不斷更新TSDF網格中各個三維小格子的訓示值的過程。對于一個三維小格子而言，其訓示值是從不同視角投影過來的資訊共同作用的結果（權重求和）。得到了最終的TSDF網格後，就可以利用計算機圖形程式完成地圖的渲染，得到一個表面完整的三維模型。此外，利用光線追蹤技術（ray casting），還可以得到在不同的位姿下，從該相機視角所看到的全局模型的輪廓。

surfel是一種類似于點雲的表征方式。對于每一個三維點，除了三維空間坐标、RGB資訊和表面法線資訊之外，它還包含了其表面半徑、初始時間戳（timestamp）、最後更新的時間戳、以及權重值（和跟相機光心的距離有關，距離越遠，權重越小）。每個surfel都描述了一個機關表面的資訊，該機關表面是一個圓形表面，且對于一個surfel集合而言，其所有的機關表面剛好可以完整地覆寫重建物體的表面。基于surfel的稠密建圖，其實就是将稀疏建圖的點雲表征方式換成surfel表征方式，在得到最終的全局地圖的surfel資訊後，再利用計算機圖形程式完成地圖的渲染，進而得到一個表面完整的三維模型。

稠密建圖由于計算量較大且易于并行化，多搭配GPU使用。

優化後端：

SLAM的優化後端完成的工作主要是對視覺前端得到的不夠準确的相機位姿和重建地圖進行優化微調。它可與視覺前端分開進行，作為一個離線操作，在近年來的一些SLAM方法中，也可以與視覺前端融為一體進行。

在視覺前端中，不管是進行位姿估計還是建圖，都是利用相鄰幀之間的關系來完成的，這種依賴局部限制且不停地鍊式進行的算法，必将導緻優化誤差逐幀累積，最終産生一個較大的誤差漂移。是以，後端優化的思路就是從全局（整個相機運動過程）中選取一些關鍵幀，利用這些關鍵幀之間的關系建立起時間和空間跨度更大的、需要同時滿足的全局限制，以優化之前得到的不夠準确的各幀的相機位姿，實際上就是完成一個Bundle Adjustment。該全局優化問題可以通過建立和優化位姿圖（pose graph）來求解。位姿圖是以關鍵幀的全局位姿（被優化的參數）作為圖的節點，以關鍵幀之間的相對位姿誤差（對關鍵幀進行配準得到的相對位姿，與通過關鍵幀的全局位姿計算出的相對位姿，兩者之間的誤差）作為圖的邊的權重，通過令整個圖的所有邊的權重值總和最小，來優化得到每個圖節點的值。這樣可以使得本來單方向漂移的誤差被攤到整個過程中，類似于完成對一個運動過程的估計的平滑（本質上的目的與卡爾曼濾波算法一緻）。

圖優化也是一個非線性優化問題，同樣地，可以通過高斯牛頓法和LM算法來求解。

在完成了圖優化後，關鍵幀的位姿得到了校正，是以就可以利用這些關鍵幀去對其相鄰的數幀也進行位姿的校正。而在完成了所有幀的位姿校正後，也就可以利用這些校正後的位姿去重建更精确的地圖。

在相機的運動過程中，有時候會重複地觀測到以前觀測過的區域（revisited areas），而由于視覺前端帶來的誤差漂移，會導緻在再次對這些區域進行建圖時，會與先前在同一區域建圖的結果不重合，即出現了重影現象。是以在後端優化中，另一個重要的工作是進行閉環（loop closure）檢測。所謂閉環檢測，其實就是判斷在相機的運動軌迹上，有沒有觀測到先前觀測過的區域，而這些區域，就是閉環點。對閉環點的檢測，也是通過對每一個到來的目前幀，都與各個曆史關鍵幀進行比對來完成的：如果存在有一個曆史關鍵幀與目前幀相似，即說明目前幀是一個閉環點。

用于閉環檢測的常用算法主要有：詞袋模型（bag-of-words model）和随機蕨算法（randomized fern method）。

詞袋模型首先對一個圖像幀上的描述子進行聚類：一個關于描述子的聚類稱之為一個詞，所有不同的詞的集合稱之為字典。對于在字典中和在單幀圖像中出現頻率較高的詞，給予其較高的權重。當對比兩個圖像幀之間的相似度時，隻要在這兩個圖像幀上，逐個詞地進行權重對比即可，常用的用于衡量權重差異的尺度為L1範數。在逐詞對比的過程中，首先需要從字典中按順序檢索出每一個用于幀間對比的詞，這是通過K-D樹來完成搜尋的。

随機蕨算法是對一個圖像幀進行稀疏編碼（sparse encoding），即在圖像幀的采樣像素點上進行資訊編碼：一個像素點被稱為一個蕨葉（fern），該像素點上的RGBD通道被稱為該蕨葉上的一組節點（node），節點值都是binary的，即在每個節點上都有一個給定的門檻值，該門檻值将确定該節點值是0還是1。于是一個圖像幀就可以用一個蕨叢來表征其資訊。在随機蕨算法中，對于每個蕨葉都會建立起一個查找表，且查找表的每一行将對應這個蕨葉的一種編碼結果。當每擷取到一個新的圖像幀，就将該圖像幀的ID登記到描述其資訊的蕨叢的各個蕨葉所對應的查找表的特定行上。如果有兩個圖像幀，其ID在超過某個給定門檻值的數量的查找表裡，都同時出現在同一行，則說明這兩個圖像幀相似，新擷取到的這個圖像幀是一個閉環點。反之，如果一個新擷取到的圖像幀，與所有的曆史幀的相似度都足夠小，則将該圖像幀作為一個關鍵幀添加到關鍵幀的資料庫中用于後續的閉環檢測。随機蕨算法還可以在後端優化中完成位姿追蹤失敗後的重定位（re-localization）：在關鍵幀資料庫中檢索與目前幀相似度最高的幾個關鍵幀，并利用它們與目前幀的相似度作為權重，對這些關鍵幀對應的位姿進行權重求和，作為對目前幀的位姿的一個估計。

在檢測出閉環點之後，就可以利用閉環點建立起一個跨度較大的全局限制，其實就是将視覺前端中的單向鍊式限制的頭尾相連，并根據頭尾相連處的位姿必須相等作為一個自洽條件，進而将這個限制條件回報到閉合後的整個環式結構的優化中，使得每個曆史幀的位姿優化不僅依賴于其過去的圖像幀的限制，也依賴于其未來的圖像幀的限制。這樣的優化過程魯棒性更好，因為對不同的優化方向作出貢獻的噪聲将借由多限制條件在一定程度上互相抵消，誤差漂移現象也是以得到改善。

SLAM的發展：

SLAM近年來發展衍生出的一些其他類型的變種有：非剛性SLAM，多目标模型SLAM，動态SLAM，語義SLAM等。

SLAM的一些開源項目：

在項目代碼方面，目前比較具有代表性的一些開源SLAM方案有：

Dense SLAM: KinectFusion， ElasticFusion，BundleFusion，InfiniTAM，MaskFusion

Semi-Dense SLAM: LSD-SLAM

Sparse SLAM: ORB-SLAM