從零開始一起學習SLAM | 給點雲加個濾網

本文閱讀時間約6分鐘，結合PCL官網吸收更佳

對VSLAM和三維重建感興趣的在公眾號選單欄回覆“三維視覺”進交流群。

小白：師兄，上次你講了點雲拼接後，我回去費了不少時間研究，終於得到了和你給的參考結果差不多的點雲，不過，這個點雲“可遠觀而不可近看”，放大了看就只有一個個稀疏的點了。究竟它能幹什麼呢？

師兄：這個問題嘛。。。基本就和SLAM的作用一樣，定位和建圖
小白：定位好理解，可是師兄說建圖，這麼稀疏的地圖有什麼用呢？
師兄：地圖分很多種，稀疏的，稠密的，還有半稀疏的等，你輸出的這個稀疏的地圖放大了看就是一個個離散的空間點，不過我們可以把它變成連續的稠密的網格，這個過程也叫點雲的網格化
小白：哇塞，聽起來好高大上呢，具體怎麼做呢？
師兄：點雲網格化需要對點雲進行一系列處理，今天我們先說說點雲處理流程的第一步，叫做點雲濾波

為什麼要對點雲濾波？

小白：濾波是什麼鬼？
師兄：濾波最早來自在數字訊號處理裡的概念，你可以理解為是一個過濾器，是對點雲的一種預處理方法
小白：哦哦，想起來中學學的濾紙，就是可以過濾掉雜質那種
師兄：哈哈，對，這個很形象了，你看下面這個圖，左側就是原來的點雲，右側是經過濾波後濾掉的“雜質”

小白：前三點還能勉強理解，這第四點中點雲中噪聲資料從哪裡來的呢？
師兄：這個很多因素啦！
一方面來自裝置。比如我們用鐳射掃描器、RGB-D相機等裝置獲取點雲資料時，由於裝置精度，電磁波的衍射特性等都會引入噪聲的。
另一方面來自環境因素帶來的影響，比如被測物體表面性質發生變化。
還有一個重要的方面就是操作者經驗帶來的影響，比如在處理點雲資料拼接配準等操作過程中引入的一些噪聲等。

小白：嗯嗯，原來噪聲資料這麼容易混進來啊。那怎麼去掉他們呢？或者說怎麼樣濾波呢？說濾波好像顯得更專業一點哈
師兄：點雲中的噪聲點對後續操作的影響比較大。就像蓋房子一樣，地基有很多瑕疵，如果不加以處理最終可能會導致整個房子坍塌的。不過別擔心，PCL中有一個專門的點雲濾波模組，可以將噪聲點去除，還可以進行點雲壓縮等操作，非常靈活實用，例如：雙邊濾波，統計濾波，條件濾波，隨機取樣一致性濾波等。這樣才能夠更好的進行配準，特徵提取，曲面重建，視覺化等後續應用處理。
小白：那太好啦，PCL都幫我們想到啦，我迫不及待的想要實踐一下啦。具體怎麼操作呢？
師兄：濾波模組主要是呼叫一些封裝好的濾波函式，然後根據需要設定一下引數，還是很直觀的。

一般來說，濾波對應的方案有如下幾種：
（1）按照給定的規則限制過濾去除點
（2） 透過常用濾波演算法修改點的部分屬性
（3）對資料進行下采樣

小白：哦哦，這麼多函式啊，哪裡有這個濾波函式大全呢？方便我需要的時候去查查用哪個的那種？

師兄：有的，PCL中關於點雲濾波的所有函式都在這裡：

下面我們舉兩個例子介紹一下，簡單的熟悉一下濾波的過程

點雲下采樣

師兄：我先說一下資料下采樣吧，這個最簡單
小白：師兄，能不能問下為啥要下采樣？我有強迫症，每做一件事情前都想知道原因。。。
師兄：嗯，這個不算強迫症啦，是個好習慣！瞭解原因了就知道什麼時候用嘛！我舉個例子，比如我們上次點雲融合，一張640x480 的Depth圖，假如每個地方都有深度值，可以轉化為30萬個點組成的點雲，如果有幾十張上百張圖這樣暴力融合，那這個融合的點雲會越來越大，儲存、操作都是個大問題！

小白：是不是相當於我有個大容量的樣本，我按一定的規則從裡面抽取有代表性的樣本，可以代替原來的樣本，是這樣嗎？
師兄：對，理解的很到位。這個下采樣PCL中有專門的類，叫做

class  pcl::ApproximateVoxelGrid< PointT >

它比較適合對海量的點雲在處理前進行資料壓縮，就像我們上次講的點雲融合後的資料那樣，而且可以在特徵提取等處理中選擇合適的體素（voxel）大小等引數，提高演算法效率。該函式對輸入的點雲資料建立一個三維體素柵格，每個體素內用體素中所有點的重心來近似顯示體素中其他點，這樣該體素內所有點都用一個重心點最終表示。它的優點是可以在下采樣的時候儲存點雲的形狀特徵。
看下面是它的結果，左邊是待處理的右邊是處理前，右邊是處理後

小白：看起來確實形狀保持的挺好，這個怎麼程式設計實現？

師兄：其實關鍵就下面幾行程式碼

pcl::VoxelGrid<PointT> downSampled;  //建立濾波物件
downSampled.setInputCloud (cloud);            //設定需要過濾的點雲給濾波物件
downSampled.setLeafSize (0.01f, 0.01f, 0.01f);  //設定濾波時建立的體素體積為1cm的立方體
downSampled.filter (*cloud_downSampled);           //執行濾波處理，儲存輸出

每行程式碼的意義都註釋好了，應該不難理解
小白：看起來好像也不麻煩
師兄；嗯，上面只是經常用的成員函式，還有一些其他成員函式，我這裡只重點介紹兩個
一個是

setLeafSize( float lx, float ly, float lz)

setLeafSize後面的三個參數列示體素柵格葉大小，分別表示體素在XYZ方向的尺寸，以米為單位，上面就是設定為長寬高都為1cm
另外一個是設定是否對所有的欄位進行下采樣，成員函式為

setDownsampleAllData(bool downsample)

小白：師兄，這個欄位是啥意思？
師兄：哦，不好意思，忘記解釋了。我們知道點雲有不同的型別，比如有的是 PointXYZ，有的是PointXYZRGB，還有其他型別，也就是一個點包含多種不同資訊，比如空間位置XYZ，顏色資訊RGB，或者強度資訊等，如果想要對所有資訊（欄位）下采樣則設定為true，只對XYZ下采樣的話設定為false

如何充分了解每個類的功能?

小白：嗯，瞭解啦，不過我有個問題，我想要用這個下采樣類時怎麼知道它有哪些函式可以用呢？
師兄：這個好問題，PCL官網上一般都有例程，但是例程包含的成員函式是很有限的，所以如果你想要了解這個濾波模板類的所有功能，或者說內聯成員函式的話，最好的辦法就是去官網查詢。我們還是以pcl::ApproximateVoxelGrid 模板類為例進行說明

第一步，登入PCL API documentation

第二步：在右上角搜尋框內輸入我們要查詢的模板類名稱，這裡我們輸入ApproximateVoxelGrid，如下所示，會自動跳出來匹配的結果

第三步：選擇你要找的類名，點選進入，就到了下面這個介面，列出了所有的成員變數和成員函式，點選每個成員函式，會跳到對應的解釋介面

小白：授人以魚不如授人以漁，謝謝師兄！以後我可以自己查函式啦

去除點雲的離群點

師兄：剛才下采樣只是萬里長征第一步，下面說一下去除離群點方法。
小白：等下，師兄，什麼是離群點啊？
師兄：哦，忘了解釋這個術語了，抱歉，我簡單說一下，離群點對應的英文是outliers，也叫外點，就是明顯偏離“群眾”的點，比如我們用鐳射掃描一面平坦的牆壁，正常情況下得到的應該是差不多也位於同一個平面的點雲，但是由於裝置測量誤差等原因，會產生少量脫離群眾的空間點，離本來的牆壁過遠，我們就叫這部分點為離群點。

小白：哈哈，離群點就是脫離群眾的壞點，明白啦！不過這些點不是很少嗎？有必要趕盡殺絕嗎？
師兄：“趕盡殺絕”，很形象的比喻！哈哈，還是很有必要的，因為離群點會使區域性點雲特徵(如表面法線或曲率變化)的估計複雜化，從而導致錯誤的值，從而可能導致點雲配準失敗。而且這些離群點還會隨著積累進行傳導，不早點消滅會有很大隱患的。

小白：原來危害這麼大，那師兄趕快告訴我該怎樣消滅它們吧！
師兄：好，我這裡列舉兩個常用的去除離群點的類，第一個類叫做 StatisticalOutlierRemoval ，顧名思義，使用統計分析技術，從一個點雲資料中集中移除測量噪聲點，也就是離群點啦！

小白：聽起來很高大上啊，那具體是什麼統計分析技術呢？
師兄：主要是對每個點的鄰域進行統計分析，剔除不符合一定標準的鄰域點。具體來說，對於每個點，我們計算它到所有相鄰點的平均距離。假設得到的分佈是高斯分佈，我們可以計算出一個均值 μ 和一個標準差 σ，那麼這個鄰域點集中所有點與其鄰域距離大於μ + std_mul * σ 區間之外的點都可以被視為離群點，並可從點雲資料中去除。std_mul 是標準差倍數的一個閾值，可以自己指定。

小白：嗯，聽起來還是挺科學的，槍打出頭鳥嘛！師兄，這個原理我懂啦，重點是怎麼程式設計呢？
師兄：這裡有個示例程式碼

pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;   //建立濾波器物件
sor.setInputCloud (cloud);                           //設定待濾波的點雲
sor.setMeanK (50);                               //設定在進行統計時考慮的臨近點個數
sor.setStddevMulThresh (1.0);                      //設定判斷是否為離群點的閥值，用來倍乘標準差，也就是上面的std_mul
sor.filter (*cloud_filtered);                    //濾波結果儲存到cloud_filtered

小白：和前面下采樣的形式很像哎
師兄：嗯，PCL都幫你寫好了，你只需要建立物件，設定引數就行啦。
小白：師兄可以稍微解釋一下上面程式碼嗎？
師兄：好，這個應該不難看懂。你看，我們先建立統計分析濾波器，然後設定濾波器輸入是 cloud，也就是我們待處理的點雲，然後設定對每個點分析的臨近點的個數設定為50 ，並將標準差的倍數設定為1，  這意味著如果一個點的距離超出了平均距離加上一個標準差以上，則該點被標記為離群點，並將它移除。最後統計分析濾波後，輸出的結果就是cloud_filtered
小白：師兄這麼一解釋感覺容易理解多了，這個方法效果怎麼樣？

師兄：效果還是挺不錯的，你看下圖展示了稀疏離群值分析和移除的效果:原始資料集顯示在左邊，結果集顯示在右邊。圖中紅色表示濾波前的平均k近鄰距離，綠色表示濾波後的平均k近鄰距離，可以看到毛刺明顯少了很多。

小白：嗯嗯，效果明顯，看的見！
師兄：另外，還有一個比較簡單常用的方法就是根據空間點半徑範圍臨近點數量來濾波，對應的類名是 RadiusOutlinerRemoval，這個很容易理解，它的濾波思想非常直接，就是在點雲資料中，設定每個點一定半徑範圍內周圍至少有足夠多的近鄰，不滿足就會被刪除。
小白：這個半徑範圍是怎麼定的？
師兄：因為空間點的具體座標都是知道的，所以我們可以很方便的計算某個點和它周圍所有點的歐氏距離，這些都是以米為單位的，可以直接指定具體的數值，對於三維建模很實用。

下圖就是該類的篩選方法。比如你指定了一個半徑d，然後指定該半徑內至少有1個鄰居，那麼下圖中只有黃色的點將從點雲中刪除。如果指定了半徑內至少有2個鄰居，那麼黃色和綠色的點都將從點雲中刪除。

pcl::RadiusOutlierRemoval<pcl::PointXYZ> pcFilter;  //建立濾波器物件
pcFilter.setInputCloud(cloud);             //設定待濾波的點雲
pcFilter.setRadiusSearch(0.8);               // 設定搜尋半徑
pcFilter.setMinNeighborsInRadius(2);      // 設定一個內點最少的鄰居數目（見上面解釋）
pcFilter.filter(*cloud_filtered);        //濾波結果儲存到cloud_filtered

小白：確實用法都差不多，我按照前面師兄講的方法充分了解每個類的功能，就會用啦！
師兄：嗯，今天篇幅已經比較長了，雖然感覺才講了一點，下次我們再講網格化吧！
小白：好，那下次再聊啦，謝謝師兄！

程式設計練習

給定一個融合後的點雲（結果來自《從零開始一起學習SLAM | 你好，點雲》），請先對其進行下采樣，再進行濾波，最後輸出濾波後的結果及被濾掉的離群點。

在計算機視覺life公眾號選單欄回覆“濾波”可以獲得程式碼框架及待處理點雲。對視覺SLAM和三維重建感興趣的讀者回復 “三維視覺” 進微信群交流。

輸入點雲如下：

如果你進行了濾波，濾掉的噪音大概如下，你發現什麼問題了嗎？

本文參考：PCL官網