PCL中3D點云特征描述與提取（一）

1 特征描述與提取的概念及相關演算法
- 1.1 3D形狀內容描述子
- 1.2 旋轉影像（Spin Images）
- 1.3 PCL中特征描述與提取模塊及類
2 點云特征描述與提取入門級實體決議
- 2.1 PCL中描述三維特征相關基礎
- - 2.1.1 理論基礎
  - 2.1.2 輸入點云呼叫習慣
- 2.2 估計一個點云的表面法線
- - 2.2.1 理論基礎
  - 2.2.2 選擇合適的尺度
  - 2.2.3 法線估計實體詳解
  - 2.2.4 使用OpenMP加速法線估計
- 2.3 使用積分圖進行法線估計

??3D點云特征描述與提取是點云資訊處理中最基礎也是最關鍵的一部分，點云的識別、分割、重采樣、配準、曲面重建等處理的大部分演算法，都嚴重依賴特征描述與提取的結果，從尺度上來分，一般分為區域特征描述和全域特征描述，例如區域的法線等幾何形狀特征的描述，全域的拓撲特征描述，都屬于3D點云特征描述與提取范疇，在PCL中，目前已有很多基本的特征描述子與提取演算法，相信在PCL的快速發展下，將來會集成和添加更多特征描述子和提取演算法，

??本章首先對涉及的部分點云特征描述與提取的概念進行簡介，由于特征描述子和提取演算法的多樣性，和實體相關的概念在后面結合實體也進行了詳細介紹；其次對PCL的特征描述與提取相關模塊及類進行簡單介紹；最后通過應用實體來展示如何對PCL中特征描述與提取相關模塊進行靈活運用，例如法線估計、各種點特征描述子的提取方法等，

1 特征描述與提取的概念及相關演算法

1.1 3D形狀內容描述子

??利用描述子建立曲面間的對應點在3D物體識別領域有廣泛應用，采用一個向量描述曲面上指定點及其鄰域的形狀特征，通過匹配向量的值來建立不同曲面間點的對應關系，此向量即為指定點的描述子，3D形狀內容描述子構造簡單，辨別力強，且對噪聲不敏感，其構造方法為：在以指定點p為中心的球形支撐域內，沿徑向、方向角和俯仰角3個坐標方向劃分成網格，統計落入網格內的點數，構造向量V，V的每個元素與支撐域內的一個網格對應，元素的值為對應網格中的點數，向量V即為點p的描述子，3D shape context網格劃分如下圖所示，詳細內容請參考[Andrea Frome, Daniel Huber, Ravi Kolluri and Thomas Bulow, Jitendra Malik: Recognizing Objects in Range Data Using Regional Point Descriptors, In proceedings of the 8th European Conference on Computer Vision (ECCV), Prague, May 11-14, 2004]，
在這里插入圖片描述

3D shape context 網格劃分

1.2 旋轉影像（Spin Images）

??旋轉影像最早是Johnson提出的特征描述子，主要用于3D場景中的曲面匹配和模型識別，如下面第一幅圖所示，在模型表面上，存在頂點 p p p 和其法向量 n n n 定義的二維基，以及切平面 P P P，假設模型上任意頂點 x x x ，現定義 α \alpha α 為 x x x 在平面 P P P 上投影點與 p p p點的距離，規定其值取大于零的實數， β \beta β 為 x x x 與其在平面 P P P 上投影點之間的距離，按照向上或向下規定其有正負之分，點 p p p 的旋轉影像則為，將除 p p p 點外其他模型上的頂點在 P P P 上的投影（ α i \alpha _{i} αi?， β i \beta _{i} βi?），其中 i i i 表示頂點的一維索引，將（ α i \alpha _{i} αi?， β i \beta _{i} βi?）統計得到二維直方圖即為點 p p p 的旋轉影像，影像的坐標由 α \alpha α 和 β \beta β 而定，強度為（ α \alpha α， β \beta β）落在同一統計區間的點的統計個數，下面第二幅圖所示模型表面三個點的旋轉影像可幫助大家理解，
在這里插入圖片描述

旋轉影像生成示意圖

在這里插入圖片描述

模型表面三個點的旋轉影像

1.3 PCL中特征描述與提取模塊及類

??PCL中pcl_features庫提供了特征描述與提取相關的基本資料結構與演算法，目前PCL內部的特征提取演算法包含基礎和最新的點云或曲面模型相關的描述子實作，包括法線估計、多種基于近鄰的區域描述算子、基于視角的全域描述算子等等，其依于common、search、kdtree、octree、range_image模塊，類和函式的介面說明受篇幅所限，請感興趣的讀者自行查閱相關資料，或者查看官網，

2 點云特征描述與提取入門級實體決議

2.1 PCL中描述三維特征相關基礎

??本小節介紹點云庫（PCL）中的三維特征描述子作業原理，以及在pcl::feature模塊中類的通用呼叫習慣，

2.1.1 理論基礎

??在原始表示形式下，點的定義是用笛卡爾坐標系坐標 x x x， y y y， z z z 相對于一個給定的原點來簡單表示的三維映射系統的概念，假定坐標系的原點不隨著時間而改變，這里有兩個點 p 1 p_{1} p1? 和 p 2 p_{2} p2? ，分別在時間 t 1 t_{1} t1? 和 t 2 t_{2} t2? 捕獲，有著相同的坐標，對這兩個點做比較其實是屬于不適定問題(ill-posed problem)，因為雖然相對于一些距離測度（如：歐幾里得度量）它們是相等的，但是它們取樣于完全不同的表面，因此當把它們和鄰近的其他環境中的點放在一起時，它們表達著完全不同的資訊，這是因為在 t 1 t_{1} t1? 和 t 2 t_{2} t2? 之間區域環境有可能發生改變，一些獲取設備也許能夠提供取樣點的額外資料，例如強度或表面反射率等，甚至顏色，然而那并不能完全解決問題，單從兩個點之間來對比仍然是不適定問題，由于各種不同需求需要進行對比以便能夠區分曲面空間的分布情況，應用軟體要求更好的特征度量方式，因此作為一個單一物體的三維點概念和笛卡爾坐標系被淘汰了，出現了一個新的概念取而代之：區域描述子（local descriptor），文獻中對這一概念的描述有許多不同的命名，如：形狀描述子（shape descriptor）或幾何特征（geometric features），本文中剩余部分都統稱之為點特征表示（point feature representations），通過包括周圍的鄰域，特征描述子能夠表征采樣表面的幾何性質，它有助于解決不適定的對比問題，如下圖所示，理想情況下，相同或相似表面上的點的特征值將非常形似（相對特定度量準則），而不同表面上的點的特征描述子將有明顯差異，下面幾個條件，通過能否獲得相同的區域表面特征值，可以判定點特征表示方式的優劣，

?? ? \bullet ? 剛體變換（rigid transformations）—— 即三維旋轉和三維平移變化不會影響特征向量F估計，即特征向量具有平移旋轉不變性，
?? ? \bullet ? 改變采樣密度（varying sampling density）—— 原則上，一個區域表面小塊的采樣密度無論是大還是小，都應該有相同的特征向量值，即特征向量具有抗密度干擾性，
?? ? \bullet ? 噪音（noise）—— 資料中有輕微噪音的情況下，點特征表示在它的特征向量中必須保持相同或者投其相似的值，即特征向量對點云噪聲具有魯棒性，

在這里插入圖片描述

點特征描述子示意圖

??通常，PCL中特征向量利用快速kd tree查詢，使用近似法來計算查詢點的最近鄰元素，有兩種常用的查詢型別，
??（1）決定一個查詢點的k鄰域元素（k為用戶已給引數）（也稱為k-搜索）
??（2）在半徑r的圍內，確定一個查詢點的所有鄰元素（也稱為半徑-搜索）

2.1.2 輸入點云呼叫習慣

??因為所有點云庫中的類都繼承來自基類pcl::PCLBase，pcl::Feature類接受以下兩種不同方式的輸入資料，
??（1）一個完整的點云資料集，由setInputCloud (PointCloudConstPtr &) 給出——此函式必須設定，這樣后續特征算子才能正常計算，任何可以進行特征描述子估計的類，為給定的輸入點云中的每個點估計一個特征向量，
??（2）點云資料集的一個子集，由setInputCloud (PointCloudConstPtr &) 和setIndices (IndicesConstPtr &) 給出——后面的setIndices 函式為可選設定，如果傳入IndicesConstPtr 引數，則任何可以進行特征估計的類將為給定輸入點云中的索引對應的點估計一個特征，默認情況下，如果沒有給出一組索引，點云中的所有點參與計算，

??此外，通過一個附加呼叫程式，可以明確指定搜索時使用的點鄰域集合setSearchSurface (PointCloudConstPtr &)，這個呼叫是可選的，當搜索點鄰域集合未給出時，則輸入點云資料為默認的搜索空間，因為總是需要 setInputCloud ()，所以我們可以使用<setInputCloud(), setIndices(), setSearchSurface()>來創建四個組合，假如我們有兩個點云， P = { p 1 , p 2 , . . . , p n } P = \left \{ p_{1}, p_{2},..., p_{n}\right \} P={p1?,p2?,...,pn?}和 Q = { q 1 , q 2 , . . . , q n } Q = \left \{ q_{1}, q_{2},..., q_{n}\right \} Q={q1?,q2?,...,qn?}，如下圖所示，則表示了所以四種情況：從左到右邊依次為，未指定索引和搜索點云集合、只指定了索引、只指定了搜索點云集合、指定了索引和搜索點云集合，

點云輸入組合

?? ? \bullet ? setIndices() = false, setSearchSurface() = false —— 毫無疑問這是點云庫中最常用的情況，用戶只需要輸入一個單一的點云資料集，并且為點云中的所有點估計一特征向量，不論一組索引和（或）搜索點云是否給定，都不希望保存不同的實作副本，無論何時，即使indices = false，PCL都會創建一組內部索引(為 std::vector<int>)，這個索引集是指向整個資料集的（indices=1..N，N是點云中點的數目），上述與圖中最左邊的情況對應，首先，我們估計了 p 1 p_{1} p1?的最近鄰元素，然后是 p 2 p_{2} p2?的最近鄰元素，以此類推，直到我們估計完 P P P中的所有點，

?? ? \bullet ? setIndices() = true, setSearchSurface() = false —— 如前面所提到的，特征估計方法只計算已給索引的點的特征，對應上圖的第二種情況，這里，我們假設 p 2 p_{2} p2?的索引不在已給的索引向量中，因此在 p 2 p_{2} p2?點處，沒有估計鄰元素或者特征向量，

?? ? \bullet ? setIndices() = false, setSearchSurface() = true —— 如第一種情況，對所有已給點進行特征向量估計，但是，在setSearchSurface()中給出的采樣面點云將用來為輸入點獲取最近鄰元素，而不是輸入點云本身，上述對應圖中第三種情況，如果 Q = { q 1 , q 2 } Q = \left \{ q_{1}, q_{2}\right \} Q={q1?,q2?}作為輸入，是不同于 P P P的另一個給出的點云， P P P是 Q Q Q的搜索表面，那么將從 P P P中計算兩個點 q 1 q_{1} q1?和 q 2 q_{2} q2?的近鄰，

?? ? \bullet ? setIndices() = true, setSearchSurface() = true —— 這種組合可能是最少見的情況，索引和搜索點云都給定，這種情況下，將使用setSearchSurface()中給出的搜索點云，只對<input, indices>中的子集進行特征向量估計，上述對應圖中最后(最右端)一種情況，這里，我們假設 q 2 q_{2} q2?的索引沒有在 Q Q Q的已給索引向量中，因此在 q 2 q_{2} q2?點處，沒有估計其鄰元素或者特征，

??在使用 setSearchSurface()時，最有用的案例是：當有一個非常密集的輸入點云資料集時，我們不想對它里面的所有點都進行特征估計，而是希望在找到的一些關鍵點處（使用pcl_keypoints中的方法進行估計），或者在點云的下采樣版本中（如：使用pcl::VoxelGrid<T>過濾而獲得的）進行特征估計，這種情況下，我們通過setInputCloud()來把下采樣后的點云/ 關鍵點傳遞給特征估計演算法，而把原始資料通過setSearchSurface()設定為搜索集合，從而提高程式的運行效率，

2.2 估計一個點云的表面法線

??表面法線是幾何體表面的重要屬性，在很多領域都有大量應用，例如：在進行光照渲染時產生符合可視習慣的效果時需要表面法線資訊才能正常進行，對于一個已知的幾何體表面，根據垂直于點表面的矢量，因此推斷表面某一點的法線方向通常比較簡單，然而，由于我們獲取的點云資料集在真實物體的表面表現為一組定點樣本，這樣就會有兩種解決方法，
??（1）使用曲面重建技術，從獲取的點云資料集中得到采樣點對應的曲面，然后從曲面模型中計算表面法線，
??（2）直接從點云資料集中近似推斷表面法線，

??本小節將針對后一種情況進行講解，已知一個點云資料集，在其中的每個點處直接近似計算表面法線，

2.2.1 理論基礎

??盡管有許多不同的法線估計方法，本教程中著重講解的是其中最簡單的一個，表述如下：確定表面一點法線的問題近似于估計表面的一個相切面法線的問題，因此轉換過來以后就變成一個最小二乘法平面擬合估計問題，
??注意：更多資訊，包含最小二乘法問題的數學方程式，可以查看相關文章，

??因此估計表面法線的解決方案就變成了分析一個協方差矩陣的特征矢量和特征值（或者PCA——主成分分析），這個協方差矩陣從查詢點的近鄰元素中創建，更具體地說，對于每一個點 P i P_{i} Pi?，對應的協方差矩陣 C C C如下：
C = 1 K ∑ i = 1 K ( P i ? P ˉ ) ( P i ? P ˉ ) T , C ? V ? j = λ j ? V ? j , j ? { 0 , 1 , 2 } C = \frac{1}{K}\sum_{i=1}^{K}\left ( P_{i} - \bar{P}\right )\left ( P_{i} - \bar{P} \right )^{T},C\cdot \vec{V}_{j}=\lambda _{j}\cdot \vec{V}_{j}, j\epsilon \left \{ 0,1,2 \right \} C=K1?i=1∑K?(Pi??Pˉ)(Pi??Pˉ)T,C?V j?=λj??V j?,j?{0,1,2}
此處， K K K 是點 P i P_{i} Pi? 鄰近點的數目， P ˉ \bar{P} Pˉ 表示最近鄰元素的三維質心， λ j \lambda _{j} λj? 是協方差矩陣的第 j j j 個特征值， V ? j \vec{V}_{j} V j? 是第 j j j 個特征向量，

??在PCL內估計一點集對應的協方差矩陣，可以使用以下函式呼叫實作：

//定義每個表面小塊的3x3協方差矩陣的存盤物件
Eigen::Matrix3f covariance_matrix;
//定義一個表面小塊的質心坐標16位元組對齊存盤物件
Eigen::Vector4f xyz_centroid;
//估計質心坐標
compute3DCentroid(cloud, xyz_centroid);
//計算3x3協方差矩陣
computeCovarianceMatrix(cloud, xyz_centroid, covariance_matrix);

??通常，沒有數學方法能解決法線的正負向問題，如上所示，通過主成分分析法（PCA）來計算它的方向也具有二義性，無法對整個點云資料集的法線方向進行一致性定向，下圖（1）中顯示出對一個更大資料集的兩部分產生的影響，此資料集來自于廚房環境的一部分，很明顯估計的法線方向并非完全一致，圖（2）部分展現了其對應擴展的高斯影像（EGI），也稱為法線球體（normal sphere），它描述了點云中所有法線的方向，由于資料集是2.5維，其只從一個單一的視角獲得，因此法線應該僅呈現出一半球體的擴展高斯影像（EGI），然而，由于定向的不一致性，它們遍布整個球體，如下圖所示，

在這里插入圖片描述

??????????（1）估計廚房環境的表面法線????????????????（2）法線球體

??如果實際知道視點 V p V_{p} Vp?，那么這個問題的解決是非常簡單的，對所有法線 V ? j \vec{V}_{j} V j? 定向只需要使它們一致朝向視點方向，滿足下面的方程式：
V ? j ? ( V p ? P i ) > 0 \vec{V}_{j}\cdot \left ( V_{p}-P_{i} \right )>0 V j??(Vp??Pi?)>0
??下圖展示了上圖中的資料集的所有法線被一致定向到視點后的結果：

在這里插入圖片描述

將所有法線一致定向到后視點的結果

??在PCL中對一個已知點的法線進行手動重新定向，可以使用如下代碼：

flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Vector4f &normal)

??注意：如果資料集是從多個捕獲視點中配準后集成的，那么上述法線的一致性定向方法就不適用了，需要使用更復雜的演算法，可以查看文章，

2.2.2 選擇合適的尺度

??如之前介紹的，在估計一個點的表面法線時，我們需要從周圍支持這個點的鄰近點著手（也稱作k鄰域），最近鄰估計問題的具體內容又提出了另一個問題“合適的尺度”：已知一個取樣點云資料集，k的正確取值是多少（k通過pcl::Feature::setKSearch給出）或者確定一個點r為半徑的圓內的最近鄰元素集時使用的半徑r該取什么值（r通過pcl::Feature::setRadiusSearch給出），這個問題非常重要，并且在一個點特征算子的自動估計時（例如用戶沒有給定閾值）是一個限制因素，為了更好地說明這個問題，以下圖示表現了選擇更小尺度（如：r值或k取相對小）與選擇更大尺度（如：r值或k值比較大）時的兩種不同效果，下面兩幅圖分別為近視圖和遠視圖，兩圖中左邊部分展示選擇了一個合理的比例因子，估計的表面法線近似垂直于兩個平面，即使在互相垂直的邊緣部分，可明顯看到邊緣，如果這個尺度取得太大（右邊部分），這樣鄰近點集將更大范圍地覆寫鄰近表面的點，估計的點特征表現就會扭曲失真，在兩個平面邊緣處出現旋轉表面法線，以及模糊不清的邊界，這樣就隱藏了一些細節資訊，

在這里插入圖片描述

表面法線估計近視圖

在這里插入圖片描述

表面法線估計遠視圖

??無法深入探究更多討論，現在可粗略假設，以應用程式所需的細節需求為參考，選擇確定點的鄰域所用的尺度，簡言之，如果杯子手柄和圓柱體部分之間邊緣的曲率是重要的，那么需要足夠小的尺度來捕獲這些細節資訊，而在其他不需要細節資訊的應用中可選擇大的尺度，

2.2.3 法線估計實體詳解

??首先創建一個作業空間normal_estimation，然后再在作業空間創建一個檔案夾src用于存放源代碼：

mkdir -p normal_estimation/src

??接著，在normal_estimation/src路徑下，創建一個檔案并命名為normal_estimation.cpp，拷貝如下代碼：

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>

int main ()
{
    /* 加載點云 */
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("../pcd/table_scene_lms400.pcd", *cloud);

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;   // 創建法線估計物件
    ne.setInputCloud (cloud);                               // 把原始點云資料傳遞給法線估計物件

    //基于給出的輸入資料集，kdtree將被建立
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());   // 創建一個空的kdtree物件
    ne.setSearchMethod (tree);      // 把kdtree物件傳遞給法線估計物件

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);         // 創建輸出資料集物件
    
    ne.setRadiusSearch (0.03);      // 使用半徑在查詢點周圍3厘米范圍內的所有近鄰元素
    
    ne.compute (*cloud_normals);    // 計演算法線，并將結果存盤到cloud_normals
    
    /* 法線可視化 */
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor (0.0, 0.0, 0.0);
    // viewer.addPointCloud(cloud);
    viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, cloud_normals);

    while (!viewer.wasStopped ())
    {
        viewer.spinOnce ();
    }

    return 0;
}

??法線估計類NormalEstimation的實際計算呼叫內部程式執行以下操作，

??（1）對點 P P P 中的每個點，得到 p p p 點的最近鄰元素
??（2）計算 p p p 點的表面法線 n n n，
??（3）檢查 n n n 的方向是否一致指向視點，如果不是則翻轉，

??視點坐標默認為（0，0，0），可以使用以下代碼進行更換：

setViewPoint (float vpx, float vpy, float vpz)

??計算單個點的法線，使用：

computePointNormal(const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, Eigen::Vector4f &plane_parameters, float &curvature)

??此處，cloud是包含點的輸入點云，indices是點的k-最近鄰元素集索引，plane_parameters和curvature是法線估計的輸出，plane_parameters前三個坐標中，以（nx，ny，nz）來表示法線，輸出表面曲率curvature通過協方差矩陣的特征值之間的運算估計得到，

【編譯和運行程式】
??在作業空間根目錄normal_estimation下，撰寫CMakeLists.txt檔案如下：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(normal_estimation)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})	

add_executable (${PROJECT_NAME}_node src/normal_estimation.cpp)
target_link_libraries (${PROJECT_NAME}_node ${PCL_LIBRARIES})

??在作業空間根目錄normal_estimation下創建一個build檔案夾，用于存放編譯程序中產生的檔案，然后執行編譯：

mkdir build
cd build
cmake ..
make

??此時，會在build檔案夾下生成一個可執行檔案normal_estimation_node，運行該可執行檔案：

./normal_estimation_node

??運行上述命令后，可以在3D可視化視窗中看到如下效果：

在這里插入圖片描述

法線估計前原始點云

在這里插入圖片描述

法線估計結果可視化

??上述代碼片段估計了輸入資料集中所有點的一組曲面法線，可以對上述代碼稍微修改，從而只為輸入資料集中的一部分點估計一組曲面法線，修改之后的代碼如下所示：

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <boost/make_shared.hpp>

int main ()
{
    /* 加載點云 */
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sub (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("../pcd/table_scene_lms400.pcd", *cloud);

    std::vector<int> indices (std::floor (cloud->size () / 2)); // 創建原始輸入點云的一個子集索引，取前50%
    for (std::size_t i = 0; i < indices.size (); ++i)
    {
        indices[i] = i;
        cloud_sub->push_back(cloud->at(i));
    }

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;   // 創建法線估計物件
    ne.setInputCloud (cloud);                               // 把原始點云資料傳遞給法線估計物件

    pcl::IndicesPtr indicesptr (new std::vector<int> (indices));
    ne.setIndices (indicesptr);     // 把子集的索引傳遞給法線估計物件

    //基于給出的輸入資料集，kdtree將被建立
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());   // 創建一個空的kdtree物件
    ne.setSearchMethod (tree);      // 把kdtree物件傳遞給法線估計物件

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);         // 創建輸出資料集物件
    
    ne.setRadiusSearch (0.03);      // 使用半徑在查詢點周圍3厘米范圍內的所有近鄰元素
    
    ne.compute (*cloud_normals);    // 計演算法線，并將結果存盤到cloud_normals

    /* 法線可視化 */
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor (0.0, 0.0, 0.0);
    // viewer.addPointCloud(cloud);
    viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud_sub, cloud_normals);

    while (!viewer.wasStopped ())
    {
        viewer.spinOnce ();
    }

    return 0;
}

??上述代碼中創建原始輸入點云的一個子集索引，取前50%，然后利用 setIndices 函式把子集的索引傳遞給法線估計物件，這樣求解出來的就是這一組子集所對應的曲面法線，結果如下所示：

在這里插入圖片描述

法線估計結果可視化（求解子集所對應的曲面法線）

??最后，下面的代碼片段將為輸入資料集中的所有點估計一組曲面法線，但將使用另一個資料集估計它們的最近鄰：

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/voxel_grid.h>
#include <boost/make_shared.hpp>

int main ()
{
    /* 加載點云 */
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("../pcd/table_scene_lms400.pcd", *cloud);

    pcl::VoxelGrid<pcl::PointXYZ> sor;      // 創建濾波物件
    sor.setInputCloud (cloud);              // 給濾波物件設定需要過濾的點云
    sor.setLeafSize (0.01f, 0.01f, 0.01f);  // 設定濾波時創建的體素大小為1cm立方體
    sor.filter (*cloud_downsampled);        // 執行濾波處理，存盤輸出到cloud_downsampled

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;   // 創建法線估計物件
    ne.setInputCloud (cloud_downsampled);                   // 把下采樣后的點云資料傳遞給法線估計物件

    // Pass the original data (before downsampling) as the search surface
    ne.setSearchSurface (cloud);            // 傳遞原始資料（下采樣之前的點云資料）作為搜索點云

    //基于給出的輸入資料集，kdtree將被建立
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());   // 創建一個空的kdtree物件
    ne.setSearchMethod (tree);      // 把kdtree物件傳遞給法線估計物件

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);         // 創建輸出資料集物件
    
    ne.setRadiusSearch (0.03);      // 使用半徑在查詢點周圍3厘米范圍內的所有近鄰元素
    
    ne.compute (*cloud_normals);    // 計演算法線，并將結果存盤到cloud_normals
    
    /* 法線可視化 */
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor (0.0, 0.0, 0.0);
    // viewer.addPointCloud(cloud);
    viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud_downsampled, cloud_normals);

    while (!viewer.wasStopped ())
    {
        viewer.spinOnce ();
    }

    return 0;
}

??在上述代碼中，利用 setInputCloud 函式把下采樣后的點云資料傳遞給法線估計物件，然后利用 setSearchSurface 函式傳遞原始資料（下采樣之前的點云資料）作為搜索點云，最終可視化結果如下所示：
在這里插入圖片描述

法線估計結果可視化（指定搜索時使用的點鄰域集合）

2.2.4 使用OpenMP加速法線估計

??對于對運算速度有要求的用戶，PCL點云庫提供了一個表面法線的附加實作程式，它使用多核/多執行緒開發規范，利用OpenMP來提高計算速度，它的類命名為pcl::NormalEstimationOMP，并且它的應用程式介面(API)100%兼容單執行緒pcl::NormalEstimation，這使它適合作為一個可選提速方法，在8核系統中，可以輕松提速6-8倍，

2.3 使用積分圖進行法線估計

??本小節我們將學習如何使用積分圖（integral images）計算一個有序點云的法線，注意該方法只適用于有序點云，

??首先創建一個作業空間normal_estimation_using_integral_images，然后再在作業空間創建一個檔案夾src用于存放源代碼：

mkdir -p normal_estimation_using_integral_images/src

??接著，在normal_estimation_using_integral_images/src路徑下，創建一個檔案并命名為normal_estimation_using_integral_images.cpp，拷貝如下代碼：

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>

int main ()
{
    /* 加載點云 */
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("../pcd/table_scene_mug_stereo_textured.pcd", *cloud);

    /* 估計法線 */
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
    ne.setMaxDepthChangeFactor(0.02f);
    ne.setNormalSmoothingSize(10.0f);
    ne.setInputCloud(cloud);
    ne.compute(*normals);

    /* 法線可視化 */
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor (0.0, 0.0, 0.0);
    viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, normals);
    while (!viewer.wasStopped ())
    {
        viewer.spinOnce ();
    }
    return 0;
}

【解釋說明】
??上述代碼的第一部分，我們從檔案中加載了一個點云存盤在點云物件，以備后續作為法線估計物件的輸入：

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile ("../pcd/table_scene_mug_stereo_textured.pcd", *cloud);

??在第二部分中，定義了存盤估計法線的點型別指標，并為創建了一個積分圖法線估計的物件ne設定物件計算時需要的引數，例如估計方法、點云等：

pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);	// 設定估計方法
ne.setMaxDepthChangeFactor(0.02f);						// 最大深度變化系數
ne.setNormalSmoothingSize(10.0f);						// 優化法線方向時考慮鄰域大小
ne.setInputCloud(cloud);								// 輸入點云，必須為有序點云
ne.compute(*normals);									// 執行法線估計，存盤結果到 normals

??以下是可使用的法線估計方法：

enum NormalEstimationMethod
{
	COVARIANCE_MATRIX,
	AVERAGE_3D_GRADIENT,
	AVERAGE_DEPTH_CHANGE
}

??COVARIANCE_MATRIX模式從具體某個點的區域鄰域的協方差矩陣創建9個積分圖，來計算這個點的法線，AVERAGE_3D_GRADIENT模式創建了6個積分圖來計算水平和垂直方向平滑后的三維梯度，并使用兩個梯度間的向量積計演算法線，AVERAGE_DEPTH_CHANGE模式只創建了一個單一的積分圖，并從平均深度變化計演算法線，

【編譯和運行程式】
??在作業空間根目錄normal_estimation_using_integral_images下，撰寫CMakeLists.txt檔案如下：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(normal_estimation_using_integral_images)

find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})

add_definitions(${PCL_DEFINITIONS})	
add_executable (${PROJECT_NAME}_node src/normal_estimation_using_integral_images.cpp)
target_link_libraries (${PROJECT_NAME}_node ${PCL_LIBRARIES})

??在作業空間根目錄normal_estimation_using_integral_images下創建一個build檔案夾，用于存放編譯程序中產生的檔案，然后執行編譯：

mkdir build
cd build
cmake ..
make

??此時，會在build檔案夾下生成一個可執行檔案normal_estimation_using_integral_images_node，運行該可執行檔案：

./normal_estimation_using_integral_images_node

??運行上述命令后，可以在3D可視化視窗中看到如下效果：

在這里插入圖片描述

積分圖法線估計示例結果

??從上圖可以看出，法線方向基本一致朝向視點，視圖視點朝向場景中的桌面，桌面上的杯子處出現平行于桌面的法線，而桌面上的點集的法線都垂直于桌面并指向點云本身獲取時的視點，利用此方法進行法線估計只適用于有序點云，對于無序點云就只能采用其他方法了，

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/310670.html

標籤：其他

上一篇：【畢設】基于openmv和arduino的人臉感應顯示的代碼

下一篇：OpenCV的影像加法運算