SLAM（重要知識點）

歐拉角的重大缺點

• 會碰到著名的萬向鎖問題，在俯仰角為正負 90°時，第一次旋轉與第三次旋轉将使用同一個軸，使得系統丢失了一個自由度（由三次旋轉變成了兩次旋轉。這被稱為奇異性問題。

李群李代數的引入

• 在做SLAM研究的時候，會涉及到對旋轉矩陣求導的問題。然而矩陣求導中多個變換矩陣相加問題很難解決，這時候需要使用矩陣李群的知識，将旋轉或者變換等矩陣以李群形式，映射到李代數上求解。旋轉矩陣對加法不封閉，就是說兩個旋轉矩陣相加并不是一個旋轉矩陣。是以在利用最小二乘求最小化誤差的過程中，不能直接對旋轉矩陣求導。但是，李代數是由向量組成的，對加法封閉，是以我們可以通過對李代數求導來間接對旋轉矩陣求導。
• SLAM的定位問題中，需要對相機的平移和旋轉進行估計，其中涉及到有關旋轉和平移的函數優化問題。使用李代數的處理方式，你可以将此類問題轉化為無限制的優化，便于求解。
• **兩種求導方式：**由于李代數由向量組成，是以對加法運算封閉。是以李代數可以根據我們熟悉的導數定義(加法)進行求導。然而這種求導的結果中有複雜的Jacobian，是以一般采用微擾法進行求導：對李群進行微擾然後對該擾動求導。結果分别為 -（Rp) ^J 和 -（Rp) ^。

批量狀态估計

• 狀态估計就是一直一段時間的運動方程和觀測資料推斷目前的機器人的狀态（位姿和路标點的位置）。我們把狀态看作服從高斯分布的随機變量，就是估計狀态的均值和協方差矩陣。批量估計問題可以轉換成最大似然估計問題，利用最小二乘進行求解。
• 卡爾曼濾波及其衍生方法都是增量的估計問題，隻跟前一時刻的狀态有關。（馬爾可夫性），視覺一般以非線性優化為主。

批量估計問題轉換成最小二乘的非線性優化問題：

• 對于機器人狀态估計的問題可以模組化為：一直機器人的運動和觀測估計機器人的狀态p(x,y|z,u)

  利用貝葉斯法則：

  p(x,y|z,u)（後驗機率） = p(z,u|x,y)*p(x,y)/p(z,u) ≈ p(z,u|x,y)p(x,y)（似然先驗）

  第一項是似然估計，第二項是先驗估計，最大化後驗估計就變成了最大化先驗和似然估計的乘積

• 最大化後驗問題直覺了解就是在已知觀測資料和運動資料現在機器人最可能在什麼位置。

  那麼似然估計的直覺了解就是在什麼狀态下最可能産生目前的觀測資料。

轉換成最小二乘優化問題：

• 觀測方程：z=h(y,x) +v,v 是觀測誤差（可以是雷射雷達或者是相機的觀測誤差）

  觀測資料z符合高斯分布O(h(y,x),Q)，Q是觀測誤差的協方差矩陣。

  是以最大化這個條件機率，轉換成最小化負對數min((z - h(y,x))Q-1(z - h(y,x)T),這個式子就轉換成了最小二乘問題了，Q是資訊矩陣。

• 通常假設各個時刻的輸入和觀測是互相獨立的，輸入和觀測也是獨立的，是以聯合機率在對數中是相乘的形式，取負對數就是相加的形式了，是以考慮多個時刻現在問題就變成了批量優化最小二乘問題。
• 這裡的資訊矩陣代表觀測的誤差的協方差矩陣的倒數，觀測越準，協方差越小，資訊矩陣越大，在優化的殘差中占的比例就越大，優化越按照它來優化，非常符合實際，越準越按照這個來優化。

幾種優化方法

• 最速下降法：目标函數在xk處泰勒展開，保留一階項，x*= - J（xk），最速下降法過于貪心，容易走出鋸齒路線，反而增加疊代次數。
• 牛頓法：二階泰勒展開，利用二次函數近似原函數。H*X= - J,牛頓法需要計算目标函數的海森陣，計算量大。規模較大時比較困難。
• 高斯牛頓法：fx進行一階泰勒展開，fx而不是Fx,高斯牛頓法用JJT來作為牛頓法中二階海森陣的近似，HX=g,在使用高斯牛頓法時，可能出現JJT*為奇異矩陣或者病态的情況，此時增量穩定性較差，導緻算法不收斂。
• 列文伯格–馬誇爾特方法（LM）：基于信賴區域理論，是由于高斯-牛頓方法在計算時需要保證矩陣的正定性，于是引入了一個限制，進而保證計算方法更具普适性。(H+入I)x=g,當 入 較小時，H占主導，近似于高斯牛頓法，較大時，入* I 占主導，接近最速下降法。

ICP-----PL-ICP

• 誤差形式不同，點到點距離與點到線距離誤差
• 收斂速度不同，ICP為一階，PL-ICP為二階收斂
• PL-ICP精度更高
• PL-ICP對初始值更敏感

貝葉斯濾波

• 已知狀态量t-1時刻的機率分布，在給定t時刻的觀測資料（Zt,Ut）的情況下估計出狀态量在t時刻的機率分布。

卡爾曼濾波（Kalman filter）

• 一種利用線性系統狀态方程，通過系統輸入輸出觀測資料，對系統狀态進行最優估計的算法。由于觀測資料中包括系統中的噪聲和幹擾的影響，是以最優估計也可看作是濾波過程。

• 線性系統的卡爾曼濾波方法：

  運動方程和觀測方程都是線性方程，卡爾曼濾波假設了馬爾可夫性，我們知道了k-1時刻的後驗分布估計目前時刻的後驗分布。可以分為一下幾步：

  第一步：利用k-1的均值和協方差預測目前時刻的先驗均值跟協方差

  第二步：利用先驗的均值和協方差的卡爾曼增益稀疏

  第三步：更新後驗的均值和協方差

• 一般SLAM系統不是線性系統，我們一般将系統做運動方程在k-1時刻的一階泰勒展開，将觀測方程在預測出的k時刻先驗處一階泰勒展開近似為線性系統，計算步驟跟線性系統一緻。
• EKF的局限性：

1. 假設了馬爾可夫性，目前資料可能跟之前的資料有關。
2. EKF在距離工作點的地方一階泰勒展開可能不準，隻相當于進行了一次疊代更新，沒有優化的方法準，優化是每優化一次再在優化之後的點處展開。
3. 需要存儲狀态量的均值和協方差，存儲量太大，不适合大型場景。
4. EKF系統沒有異常檢測機制，遇到異常值可能會發散。

拓展：EKF（擴充卡爾曼濾波）把卡爾曼濾波擴充到非線性系統中，通常的做法是，在某個點附近考慮運動方程及觀測方程的一階泰勒展開，隻保留一階項，即線性的部分，然後按照線性系統進行推導。

EKF和BA的差別：

（1）EKF假設了馬爾可夫性，k時刻的狀态隻與k-1時刻有關。

（2）非線性優化使用所有的曆史資料，全局slam。

（3）EKF做了線性化處理，在工作點處用一階泰勒展開式近似整個函數，工作點較遠處不一定成立。（4）非線性優化每疊代一次，狀态估計發生改變，會對新的估計點做泰勒展開。

• 總而言之，EKF可被看作是隻有一次疊代的BA。

基于濾波和基于優化的差別以及做法？

• slam的後端一般分為兩種處理方法，一種是EKF（擴充卡爾曼濾波）為代表的濾波方法，一種是以圖優化為代表的非線性優化（BA）方法。主流方法還是圖優化。

• 濾波方法的優缺點：

  （1）優點：在資源受限、待估計量比較簡單的情況下，EKF比較有效，經常用在雷射slam中。

  （2）缺點：存儲量和狀态量是平方增長關系（存儲的是協方差矩陣）。不适合大型場景。現階段，基于視覺的slam中特征點資料大，濾波方法效率非常低。

• 圖優化一般分為兩個任務：建構圖。機器人位姿作為頂點，位姿間的關系作為邊；優化圖。調整機器人的位姿來盡量滿足邊的限制，使得誤差最小。

BA優化

• Bundle Adjustment : 從視覺重建中提煉出最優的3D模型和相機參數（内參和外參），好似每一個特征點都會反射幾束光線，當把相機位姿和特征點位置做出最優的調整後，這些光線都收束到相機相機光心。也就是根據相機的投影模型構造構造代價函數，利用非線性優化（比如高斯牛頓或列文伯格馬誇而爾特）來求最優解，利用雅克比矩陣的稀疏性解增量方程，得到相機位姿和特征點3D位置的最優解。
• (通俗解釋)流程：先按照正常的步驟計算出3D點的坐标；然後重投影（按理說一個正向計算，一個反向計算，誤差是0才對），但是由于各種原因（不清楚具體是啥）存在了重投影（像素坐标）誤差。是以需要求MIN的誤差。求的過程中需要不斷調整3D點位置與相機外參，是以求出MIN誤差的時候，也擷取了最優的3D點位置與相機姿态。

如何對比對好的點做進一步的處理，更好保證比對效果

• 暴力比對是，确定比對最小距離，漢明距離要小于最小距離的兩倍才是正确的比對。（工程經驗，當描述子的距離大于最小距離的兩倍時，認為比對有誤）。
• 使用KNN-matching算法，令K=2。則每個match得到兩個最接近的descriptor，然後計算最接近距離和次接近距離之間的比值，當比值小于門檻值時，才作為最終match。

• RANSAC（随機抽樣一緻)，利用RANSAC找到最好的對極限制D

  1.從比對好的點中找八對點，然後計算其他比對點到極線的距離，當距離小于内點時看作内點。

  2.當内點的數目占95或者到達疊代次數k時把目前的本質矩陣作為最好的模型。

  3.以目前的本質矩陣求解出相機的運動。（使用RANSAC找到最佳單應性矩陣。由于這個函數使用的特征點同時包含正确和錯誤比對點，是以計算的單應性矩陣依賴于二次投影的準确性）

描述PnP

• Perspective-n-Points, PnP(P3P)提供了一種解決方案，它是一種由3D-2D的位姿求解方式，即需要已知比對的3D點和圖像2D點。目前遇到的場景主要有兩個，其一是求解相機相對于某2維圖像/3維物體的位姿；其二就是SLAM算法中估計相機位姿時通常需要PnP給出相機初始位姿。給出初始位姿之後就可以利用三角測量得到點的3D位置，以後就可以利用PnP求導了，工程中一般将PnP和ICP放到一個優化架構下優化，因為他們隻差一個内參矩陣。

如何選取關鍵幀

• 關鍵幀選取的名額主要有：

（1）跟蹤品質（主要根據跟蹤過程中搜尋到的點數和搜尋的點數比例）/共視特征點

（2）距離最近關鍵幀的距離是否足夠遠（空間）/運動

（3）距離上一關鍵幀的幀數是否足夠多（時間）

單目視覺slam中尺寸漂移是怎麼産生的

• 單目相機根據一張圖檔無法得出一張圖檔中物體的實際大小，同理也就無法得出運動的尺度大小，這是産生尺度漂移的根源。而在優化過程中，單目相機使用對極幾何中的三角測量原理，而三角測量中，極小的角度誤差在累積之後深度不确定都會變得很大，進而無法保證尺度一緻性。
• 單目SLAM的尺度漂移，簡單來說就是單目獲得的單張圖檔無法知道拍攝物體到相機的距離，必須通過對極幾何求的兩張圖檔的位姿，然後三角化出來尺度資訊。而這些都依賴的是兩幀圖像之間的逆深度求解。
• 任選兩張圖（設為第0幀和第1幀），先提取關鍵點并比對，再根據對極幾何求本質矩陣E，再從E中恢複R,t。這個在單目VO裡屬于初始化的部分，其中得到的t是個機關向量，因為任意縮放t，極線限制都成立。你可以對t進行任意縮放。縮放完後，設第0幀的位置為相機坐标系原點，則第1幀的位置就可以由R, t得到。雖然這個t不是真值，但是可以固定下來。然後根據兩幀的位置，可以對圖像幀0和1中比對的三維點進行三角化。（多點擷取本質矩陣；恢複R,t；并根據R,t三角化計算出僞深度； ）。
• 對之後的圖像幀，就不再是利用對極幾何求R,t了。依舊是先提取關鍵點再比對哈， 但是這次比對的上一幀的特征中，有些是已經被三角化過的，是以可以像之前答主說的用運動模型加BA求該圖像幀與上一幀的位置關系，也可以用PnP求R,t。求出來R,t之後，再三角化該幀與上一幀的沒被三角化過的比對點。
• 我們可以發現在尺度漂移就是出現在第二步。因為對于新的圖像幀，都是先計算其R,t，再利用該R,t三角化與之前圖像幀比對的點。問題就是，如果第一步算的R,t有誤差，那麼第二步三角化的三維點的深度就存在誤差，也就是說其深度會存在一個縮放。對于新來的每一幀，都會有這樣的問題。

SLAM中的綁架問題

• 綁架問題就是重定位，是指機器人在缺少之前位置資訊的情況下，如何去确定目前位姿。例如當機器人被安置在一個已經建構好地圖的環境中，但是并不知道它在地圖中的相對位置，或者在移動過程中，由于傳感器的暫時性功能故障或相機的快速移動，都導緻機器人先前的位置資訊的丢失，在這種情況下如何重新确定自己的位置。

描述特征點法和直接法的優缺點

• 特征點法

  優點：

  （1）精确，直接法屬于強假設

  （2）運動過大時，隻要比對點在像素内，則不太會引起誤比對，魯棒性好

  缺點：

  （1）關鍵點提取、描述子、比對耗時長

  （2）特征點丢失場景無法使用

  （3）隻能建構稀疏地圖

• 直接法

  優點：

  （1）省去計算特征點、描述子時間

  （2）可以用在特征缺失的場合（比如白牆）

  （3）可以建構半稠密乃至稠密地圖

  缺點：

  （1）易受光照和模糊影響

  （2）運動必須微小，要求相機運動較慢或采樣頻率較高（可以用圖像金字塔改善）

  （3）非凸性；單個像素沒有區分度

特征點法和直接法的BA有何不同

• 誤差函數不同。特征點法是重投影誤差，直接法是光度誤差
• 雅克比矩陣不同

光流和直接法有何不同：

• 光流法還是需要提取圖像中有代表性的點，圖像的特征點，隻是不需要再計算描述子了，直接通過灰階假設計算特征點像素的運動，然後通過PNP、對極幾何、ICP等方法求解相機的運動；而直接法直接通過灰階假設計算得到相機之間的運動，任何圖像的點都可以，不需要提取有代表性的點。
• 光流僅估計了像素間的平移，但沒有用相機結構,沒有考慮相機的旋轉和圖像縮放,邊界點追蹤效果差

特征比對（稀疏）和稠密比對差別

• 特征比對：

  （1）速度快，效率高，可以到亞像素級别，精度高

  （2）比對元素為物體的幾何特征，對照明變化不敏感

• 稠密比對

  （1）速度慢，效率低

  （2）對無紋理區域比對效果不理想，對光強條件敏感

EKF和BA的差別：

• 都是基于貝葉斯估計得到狀态的最大後驗估計。
• EKF假設了馬爾科夫性，認為k時刻的狀态隻與k-1時刻有關。非線性優化使用所有的曆史資料，做全體的SLAM
• EKF做了線性化處理，在固定點處用一階泰勒展開式近似整個函數，但在工作點較遠處不一定成立。非線性優化每疊代一次，狀态估計發生改變，我們會重新對新的估計點做泰勒展開，可以把EKF看做隻有一次疊代的BA。

單目，雙目，深度相機對比：

• 單目：成本低，搭建簡單，單目相機有尺度不确定性，需要專門初始化
• 雙目：不需要專門初始化，能夠計算深度，基線距離越大，測量距離越遠，可以用于室内和室外，标定較為複雜，視差計算比較消耗資源
• 深度：測量範圍窄，噪聲大，易受日光幹擾，無法測量透射材料，主要用于室内。

為什麼SLAM中常用L-M？

• G-N中的H矩陣可能為奇異矩陣或者病态矩陣（殘差函數在局部不像一個二次函數，泰勒展開不是很準确），導緻算法不收斂。而且當步長較大時，也無法保證收斂性，是以采用L-M求解增量方程，但是它的收斂速度可能較慢。L-M法可以避免線性方程組的病态和奇異問題，可以得到更穩定、更準确的增量。

RANSAC算法

• RANSAC算法的基本假設是樣本中包含正确資料(inliers，可以被模型描述的資料)，也包含異常資料(outliers，偏離正常範圍很遠、無法适應數學模型的資料)，即資料集中含有噪聲。這些異常資料可能是由于錯誤的測量、錯誤的假設、錯誤的計算等産生的。同時RANSAC也假設，給定一組正确的資料，存在可以計算出符合這些資料的模型參數的方法。RANSAC算法可以在資料存在異常時得到正确的模型。

• 優缺點：

  RANSAC算法的優點是能魯棒的估計模型參數。例如，他能從包含大量局外點的資料集中估計出高精度的參數。缺點是

  1.它計算參數的疊代次數沒有上限，如果設定疊代次數的上限，得到的結果可能不是最優的結果，甚至可能得到錯誤的結果。

  2.RANSAC隻有一定的機率得到的可信的模型，機率與疊代次數成正比。

  3.另一個缺點是它要求設定跟問題相關的門檻值，RANSAC隻能從特定的資料集中估計出一個模型，如果存在兩個（或多個）模型，RANSAC不能找到别的模型。

如何優化重投影誤差？采用什麼方法求解？如果誤比對的點重投影之後誤差很大，如何解決它對整個優化問題的影響？

• 圖優化模型，将路标點和相機位姿作為兩個節點，觀測模型作為邊，同時優化兩個變量
• SLAM中常用L-M求解，如果誤比對誤差很大可以考慮用核函數（Huber），設定魯棒和函數

雷射雷達運動畸變

• 産生原因：在機器人運動過程中，每個雷射點都在不同的基準位姿上産生（就是在不同的時刻發出雷射時機器人的位置是不同的（由于機器人自身的運動），這也是運動畸變産生的根源所在。當雷射雷達掃描的頻率比較低的時候，機器人運動帶來的雷射幀的運動誤差是不能被忽略的。
• 去除畸變的原理：去除雷射雷達運動畸變的原理是把一幀雷射雷達資料的每個雷射點對應的雷射雷達坐标轉換到不同時刻的機器人裡程計上（近似對應的裡程計的位置，達到盡可能去除畸變的目的）

• 去除運動畸變的方法：

  1.使用ICP算法：ICP算法的缺點：沒有考慮雷射的運動畸變，是以得到的雷射資料是錯誤的（計算的時候假設是正确的），進而造成點雲比對發生錯誤，通過不斷的疊代，雖然能夠收斂，能夠求解，但是存在誤差。

  2.VICP算法考慮了雷射的運動畸變，也就是考慮了機器人的運動和速度的存在。ICP算法的變種，考慮了機器人的運動，勻速運動（假設是勻速運動），進行比對的同時估計機器人的速度。VICP 方法的缺點：低頻率情況下（5Hz）,勻速運動假設不成立 ，資料預處理和狀态估計過程耦合

  3.我們盡可能的反應運動情況，實作預處理和狀态估計的解耦，我們提出了傳感器輔助的方法。使用慣性測量單元IMU（直接測量獲得角速度和線速度，但對于機器人位移和角度需要積分）或者輪式裡程計（直接測量機器人的位移和角度）。

如何求解Ax=b的問題

• A是滿秩的方陣可以直接求逆

• 1、三角分解（LU分解）

  2、LDLT分解與LLT分解（Cholesky分解）

  3、QR分解

  4、奇異值分解（SVD分解）

• 轉變成||Ax-b||2利用最小二乘疊代求解。

SIFT和SUFT的差別

• 建構圖像金字塔，SIFT特征利用不同尺寸的圖像與高斯差分濾波器卷積；SURF特征利用原圖檔與不同尺寸的方框濾波器卷積。
• 特征描述子，SIFT特征有4×4×8=128維描述子，SURF特征有4×4×4=64維描述子
• 特征點檢測方法，SIFT特征先進行非極大抑制，再去除低對比度的點，再通過Hessian矩陣去除邊緣響應過大的點；SURF特征先利用Hessian矩陣确定候選點，然後進行非極大抑制
• 特征點主方向，SIFT特征在正方形區域内統計梯度幅值的直方圖，直方圖最大值對應主方向，可以有多個主方向；SURF特征在圓形區域内計算各個扇形範圍内x、y方向的haar小波響應，模最大的扇形方向作為主方向

關于imu預積分

• 為什麼進行imu積分

  IMU的頻率遠高于Camera，而IMU獲得的是每一時刻的加速度和角速度，我們通過積分獲得兩幀之間的由IMU測出的位移和旋轉變換。傳統慣性導航的遞推算法，是在已知上一時刻的IMU狀态量（姿态和速度、位移）情況下，利用IMU測量得到的線加速度和角速度，做積分運算得到目前時刻的狀态量。

• 為什麼要進行預積分

  在基于非線性優化的VIO中，各個節點的狀态量都是估計值（此處還包括Ba和Bg）。當這些狀态量在優化算法中，會不斷調整，每次調整都需要在它們之間重新積分，傳遞IMU測量值。是以提出了IMU預積分，希望對IMU的相對測量進行處理，使它與絕對位姿解耦，或者隻要線性運算就可以進行矯正，進而避免絕對位姿被優化時進行重複積分。

傳感器時間同步

• 因為時鐘源都有鐘漂，而且每個時鐘源鐘漂不同，是以即使把各個傳感器時間戳在初始時刻對齊，運作一段時間之後，之前對齊的結果仍會偏離。解決這個問題的辦法就是在硬體上把時鐘源統一，常見的做法是做一個脈沖發生器，所有傳感器都被這個脈沖觸發，每次觸發都校正一次自己的時鐘，這樣就可以消除時鐘源的累計誤差。

scan-context介紹

Scan Context 應用于基于3D點雲的重定位和場景識别，主要思想是将場景3維資訊壓縮，将笛卡爾坐标系的資訊轉換到極坐标系下計算。優勢是高效利用場景點雲分布特征，引入"旋轉不變性"描述子，快速搜尋。

1. 給定一幀點雲，劃分成20個環，每個環分成60等份，一共1200個格了
2. 每個格子存裡面點的最大高度值(z值)，這樣一幀點雲就用一個二維圖像表示了，想象成一個帶高度的俯視圖，或者地形圖，記為scan context
3. scan context進一步計算列的均值，得到一個1x60的向量，記為rina kev;計算行的均值，得到一個20x1的向量，記為sector kev
4. 用ring key構造kd-tree，并且執行knn搜尋
5. 對于候選比對scan context，首先要左右循環偏移一下，對齊，實際會用sector key去對齊，得到一個偏移量
6. 對候選比對scan context，施加偏移量，然後作比較

魯棒核函數與RANSAC方法

• 我們構造3D-3D、3D-2D、2D-2D比對，然後據此去估計相機的運動。完美估計需要完美的比對，但實際的比對中往往存在很多錯誤。如何消除或者降低錯誤比對的影響呢？一種方法是選擇那些正确的比對進行估計（RANSAC），另一種是降低那些錯誤比對的權重（魯棒核函數）。

  

• 上述RANSAC方法進行模型估計，實際分了倆步，首先選出局内點，然後進行優化。魯棒核函數隻需要一步，直接優化求解模型參數，通過降低外點的權重，來降低資料錯誤的影響。魯棒核函數能夠保證每條邊的誤差不會大的沒邊而掩蓋其他的邊。具體方式是把原先誤差的二範數度量替換成一個沒有增長那麼快的函數，同時保證自己光滑的性質。

旋轉矩陣的特點

• 一個矩陣是旋轉矩陣，當且僅當它是正交矩陣并且它的行列式是機關一
• 旋轉矩陣是正交矩陣，如果它的列向量形成的一個Rn正交基，就是說在任何兩個列向量之間的标量積是零(正交性)而每個列向量的大小是機關一(機關向量)。
• 旋轉矩陣的逆矩陣是它的轉置矩陣
• 在三維空間中，旋轉矩陣有一個等于機關1的實特征值。