從零開始一起學習SLAM | 點雲平滑法線估計

點雲濾波後為什麼還需要平滑？

小白：師兄，師兄，上次你說的點雲濾波我學會啦，下一步怎麼把點雲變成網格啊？
師兄：濾波只是第一步，在網格化前我們還需要對濾波後的點雲進行平滑（smoothing）
小白：不是已經濾波了嗎？怎麼還要平滑啊？濾波和平滑不一樣嗎？
師兄：確實不太一樣。我們用RGB-D，鐳射掃描器等裝置掃描物體，尤其是比較小的物體時，往往會有測量誤差。這些誤差所造成的不規則資料如果直接拿來曲面重建的話，會使得重建的曲面不光滑或者有漏洞，而且這種不規則資料很難用前面我們提到過的統計分析等濾波方法消除，所以為了建立光滑完整的模型必須對物體表面進行平滑處理和漏洞修復。
你看下面左邊就是原始的掃描資料，右邊就是用最小二乘法進行表面平滑後的結果

小白：從圖上看，平滑確實效果很明顯啊，左邊杯子上黑色的是噪聲吧，右邊的結果來看經過平滑都消失了
師兄：對，除了上面說到的裝置測量誤差外，還有一種情況也需要對點雲進行平滑。就是後處理過程中，比如我們對同一個物體從不同方向進行了多次掃描，然後把掃描結果進行配準，最後得到一個完整的模型，但是你配準的結果不一定準啊，比如下圖中左側就是配準後未經過處理的結果，同一面牆壁由於配準誤差變成了“兩面牆”，並不能完全重疊，你覺得這個資料可以直接用來進行表面重建嗎？

如何透過重取樣實現點雲平滑？

師兄：點雲重取樣，我們實際上是透過一種叫做“移動最小二乘”（MLS， Moving Least Squares ）法來實現的，對應的類名叫做：pcl::MovingLeastSquares，你知道怎麼用嗎？
小白：不知道，不過我還記得我們上次師兄給我說的方法，在PCL API documentation 上查詢類名稱，就能看到類的定義和用法啦
師兄：活學活用啊，哈哈，那我們現在去查一下看看吧
小白：嗯，我查到了，這個MLS類的定義在這裡：
classpcl_1_1_moving_least_squares.html#a379330b0b1dacaa668d165f94930749c
成員函式好多啊
師兄：對，看著是很多，但是很多我們不常用的，比如我們常用的一個用於重取樣的示例程式碼如下，每行程式碼都給你註釋好了，結合上面網址看很容易理解

// 對點雲重取樣  
pcl::search::KdTree

小白：師兄，這個程式碼裡的KD-Tree是幹嘛的？
師兄：Kd-Tree是一種資料結構，是空間二分樹的一種特殊情況，可以很方便的用於進行範圍搜尋。在這裡用KD-Tree就是為了便於管理、搜尋點雲，這種結構來可以很方便的找到最近鄰點。
小白：原來如此，那上面mls.setSearchRadius (0.05) 的意思是不是就是搜尋當前點以5cm為半徑的空間中所有的點？
師兄：對的，然後把這些點用2階多項式擬合~

如何估計點雲的表面法線？

小白：師兄，現在可以網格化了嗎？
師兄：還不行。。。別急，網格化前我們還需要估計一下點雲的表面法線（normal）
小白：啊，怎麼又冒出來一個法線。。。
師兄：法線好像是中學就學過了，應該還記得平面的法線的定義吧，平面的法線是垂直於該平面的向量，如下圖所示

你看上面右邊那個圖，對於曲面來說，曲面在某點P處的法線為垂直於該點切平面（tangent plane）的向量
小白：記得呢，不過這個法線有什麼用？怎麼就突然冒出來了
師兄：法線很有用的，尤其是在三維建模中應用非常廣泛，比如在計算機圖形學（computer graphics）領域裡，法線決定著曲面與光源（light source）的強弱處理（Flat Shading），對於每個點光源位置，其亮度取決於曲面法線的方向。
小白：原來如此。不過好像平面或曲面的法線比較容易計算，方程 ax + by + cz = d 表示的平面，向量(a, b, c)
就是其法線。而我們這裡是點雲呢！怎麼算呢？
師兄：確實如此。點雲的法線計算是稍微麻煩點，一般有兩種方法：
1、使用曲面重建方法，從點雲資料中得到取樣點對應的曲面，然後再用曲面模型計算其表面的法線
2、直接使用近似值直接從點雲資料集推斷出曲面法線
這裡主要用第2種方法來近似估計點雲中每個點的表面法線。
具體來說，就是把估計某個點的表面法線問題簡化為：從該點最近鄰計算的協方差矩陣的特徵向量和特徵值的分析，這裡就不多做介紹了。PCL已經幫我們封裝好了函式啦
我們計算出來點雲的法線大概是這樣的

小白：確實是這樣啊，看來程式設計的時候要格外注意了。
師兄：法線估計的示例如下，我也給你註釋好啦

// 法線估計
pcl::NormalEstimation

本文參考：PCL官網