從零開始一起學(xué)習(xí)SLAM | 點(diǎn)云平滑法線估計(jì)

點(diǎn)云濾波后為什么還需要平滑？

小白：師兄，師兄，上次你說的點(diǎn)云濾波我學(xué)會(huì)啦，下一步怎么把點(diǎn)云變成網(wǎng)格啊？
師兄：濾波只是第一步，在網(wǎng)格化前我們還需要對(duì)濾波后的點(diǎn)云進(jìn)行平滑（smoothing）
小白：不是已經(jīng)濾波了嗎？怎么還要平滑啊？濾波和平滑不一樣嗎？
師兄：確實(shí)不太一樣。我們用RGB-D，激光掃描儀等設(shè)備掃描物體，尤其是比較小的物體時(shí)，往往會(huì)有測(cè)量誤差。這些誤差所造成的不規(guī)則數(shù)據(jù)如果直接拿來曲面重建的話，會(huì)使得重建的曲面不光滑或者有漏洞，而且這種不規(guī)則數(shù)據(jù)很難用前面我們提到過的統(tǒng)計(jì)分析等濾波方法消除，所以為了建立光滑完整的模型必須對(duì)物體表面進(jìn)行平滑處理和漏洞修復(fù)。
你看下面左邊就是原始的掃描數(shù)據(jù)，右邊就是用最小二乘法進(jìn)行表面平滑后的結(jié)果

小白：從圖上看，平滑確實(shí)效果很明顯啊，左邊杯子上黑色的是噪聲吧，右邊的結(jié)果來看經(jīng)過平滑都消失了
師兄：對(duì)，除了上面說到的設(shè)備測(cè)量誤差外，還有一種情況也需要對(duì)點(diǎn)云進(jìn)行平滑。就是后處理過程中，比如我們對(duì)同一個(gè)物體從不同方向進(jìn)行了多次掃描，然后把掃描結(jié)果進(jìn)行配準(zhǔn)，最后得到一個(gè)完整的模型，但是你配準(zhǔn)的結(jié)果不一定準(zhǔn)啊，比如下圖中左側(cè)就是配準(zhǔn)后未經(jīng)過處理的結(jié)果，同一面墻壁由于配準(zhǔn)誤差變成了“兩面墻”，并不能完全重疊，你覺得這個(gè)數(shù)據(jù)可以直接用來進(jìn)行表面重建嗎？

如何通過重采樣實(shí)現(xiàn)點(diǎn)云平滑？

師兄：點(diǎn)云重采樣，我們實(shí)際上是通過一種叫做“移動(dòng)最小二乘”（MLS， Moving Least Squares ）法來實(shí)現(xiàn)的，對(duì)應(yīng)的類名叫做：pcl::MovingLeastSquares，你知道怎么用嗎？
小白：不知道，不過我還記得我們上次師兄給我說的方法，在PCL API documentation http://docs.pointclouds.org/trunk/ 上查詢類名稱，就能看到類的定義和用法啦
師兄：活學(xué)活用啊，哈哈，那我們現(xiàn)在去查一下看看吧
小白：嗯，我查到了，這個(gè)MLS類的定義在這里：
http://docs.pointclouds.org/trunk/classpcl_1_1_moving_least_squares.html#a379330b0b1dacaa668d165f94930749c
成員函數(shù)好多啊
師兄：對(duì)，看著是很多，但是很多我們不常用的，比如我們常用的一個(gè)用于重采樣的示例代碼如下，每行代碼都給你注釋好了，結(jié)合上面網(wǎng)址看很容易理解

// 對(duì)點(diǎn)云重采樣  
pcl::search::KdTree

小白：師兄，這個(gè)代碼里的KD-Tree是干嘛的？
師兄：Kd-Tree是一種數(shù)據(jù)結(jié)構(gòu)，是空間二分樹的一種特殊情況，可以很方便的用于進(jìn)行范圍搜索。在這里用KD-Tree就是為了便于管理、搜索點(diǎn)云，這種結(jié)構(gòu)來可以很方便的找到最近鄰點(diǎn)。
小白：原來如此，那上面mls.setSearchRadius (0.05) 的意思是不是就是搜索當(dāng)前點(diǎn)以5cm為半徑的空間中所有的點(diǎn)？
師兄：對(duì)的，然后把這些點(diǎn)用2階多項(xiàng)式擬合~

如何估計(jì)點(diǎn)云的表面法線？

小白：師兄，現(xiàn)在可以網(wǎng)格化了嗎？
師兄：還不行。。。別急，網(wǎng)格化前我們還需要估計(jì)一下點(diǎn)云的表面法線（normal）
小白：啊，怎么又冒出來一個(gè)法線。。。
師兄：法線好像是中學(xué)就學(xué)過了，應(yīng)該還記得平面的法線的定義吧，平面的法線是垂直于該平面的向量，如下圖所示

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

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

// 法線估計(jì)
pcl::NormalEstimation

本文參考：PCL官網(wǎng)