LOAM演算法核心解析之特徵點的提取(一)

本篇文章接好LOAM中的特徵點提取演算法，以及其特徵點的定義方法。

LOAM演算法整體的流程與傳統的基於特徵點的視覺SLAM很相似：
第一步、提取點雲中的特徵點；
第二步、使用最近鄰演算法計算匹配點；
第三步、使用梯度下降演算法求解當前幀點雲姿態。

由於LOAM是基於機械掃描式的3D鐳射感測器的SLAM演算法，所以其特徵點提取演算法很大程度上也與機械掃描式鐳射的物理特性相關。
這裡我們假設鐳射的座標系為： x正–向前，y正–向右, z正–向上，這種座標系一般是我們比較常用的車輛座標系，其滿足右手系。

這裡有一個小知識點，如果我們繞某一個軸進行旋轉，怎樣確定哪個方向是正，哪個方向是負呢？
判斷方法:假設我們要繞x軸旋轉，我們用右手握住x軸，大拇指指向x軸的正向，則彎曲四指的方向為旋轉的正向。

一、按鐳射線進行點雲聚類

3D鐳射按線束劃分種類，當前的機械掃描式的鐳射一般包括：16線，32線，64線等。所以LOAM演算法的第一步是將接收到的3D點雲，根據其與水平面的夾角劃分到鐳射的不同線上。
LOAM對點雲的預處理首先會把點按鐳射的線束進行分割，將相同線束上的點劃分到一起。其劃分方式就是依據3D點座標，求得每個點相對與鐳射座標x-y平面的夾角，依據不同夾角劃分道不同的線束上，其公式為:
$$\theta =arctan(z,\sqrt{x^2+y^2})$$

其中$\theta$為夾角，x,y,z為鐳射座標系下點的座標。

二、特徵點篩選條件

為了提取點雲中穩定特徵點，用於定位匹配，作者認為形狀尖銳的角點、形狀平滑的角點和平面點是點雲中的特徵點。所以定義了一種特徵點的劃分演算法，具體演算法為：

計算當前點前後個5個點與當前的點的曲率(實際計算方差)，其公式為：
$$c=\frac{1}{|S|\cdot ||X_{(k,i)}^L||}||\sum _{j\in S,j\ne i}(X_{(k,i)}^L-X_{(k,j)}^L)||$$

實際程式碼中的計算第i個點的曲率的公式:
$$c=\frac{1}{2\ast n}\sum _j^{(-n,n)}(X_{(k,i)}^L-X_{(k,i-j)}^L)$$

其中, c表示計算得到的曲率；$n$ 表示計算點雲的區域，是以當前點i為中心，向前後各取n個點；k表示第k幀點，L表示是在鐳射座標下。

1. 形狀尖銳的角點
   對所有計算曲率進行排序，通過找到n（可以設定）個大麴率的top點，認為是形狀尖銳的點

2. 形狀平滑的角點
   然後提取m(通過可設定的閾值進行提取)個曲率較大的點作為形狀平滑的角點

3. 平面點
   然後找到k個(通過可設定的閾值進行提取)曲率小於閾值的點認為是平面點

這樣就可以在一幀中得到有效的點數了。而在論文中是對整個掃描進行化段，分四段，每段各取兩個邊緣點和4個平面點.

三、過濾一些異常點

通過上面的特徵提取的特徵點仍然會存在一些不穩定的點，為了保證演算法的問題去要對其進行識別，並剔除。

異常點定義為：

1. 該點的周圍的點儘量不要被再被取到，這樣可以使整體的特徵點分佈更加的平均
2. 該點不能與雷達掃描束過於平行，這樣也會出現問題

具體的表現形式如圖：