PCL中點雲濾波器的使用（Filtering In PCL）

1 開發環境配置

（1) VS2015 & PCL1.8.1

（2) 配置PCL頭文件及庫目錄

Include:
$(PCL_ROOT);
$(PCL_ROOT)\include\pcl-1.8;
$(PCL_ROOT)\3rdParty\Eigen\eigen3;
$(PCL_ROOT)\3rdParty\FLANN\include;
$(PCL_ROOT)\3rdParty\VTK\include\vtk-8.0;
$(PCL_ROOT)\3rdParty\Boost\include\boost-1_64;

lib:
$(PCL_ROOT)\lib\*.lib
$(PCL_ROOT)\3rdParty\VTK\lib\*.lib
$(PCL_ROOT)\3rdParty\FLANN\lib\*.lib
$(PCL_ROOT)\3rdParty\Boost\lib\*.lib

2 PCL 濾波器介紹

PCL中基本濾波器主要有以下六種：

（1）PassThrough filter

PassThrough filter是在特定的維度（x, y, z座標軸中的某一個）對點雲進行濾波，即移除給定範圍的內部點雲或外部點雲。使用方式如下代碼所示，

	// Create the filtering object
	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(580.0, 900.0);
	//pass.setFilterLimitsNegative (true);
	pass.filter(*cloud_filtered);

以上代碼主要是創建PassThrough filter對象並設置其參數，此處濾波座標系設置爲Z軸，通過setFilterLimits來選取( 580<Z<900 )內的點雲。當setFilterLimitsNegative參數設爲True時，則選取（ Z<580 || Z >900 ）內的點雲，程序最後一行filter的參數指定濾波後的點雲。下圖爲選取點雲的簡單示意圖：

官方示例給出結果爲：

Cloud before filtering:
	0.352222 -0.151883 -0.106395
	-0.397406 -0.473106 0.292602
	-0.731898 0.667105 0.441304
	-0.734766 0.854581 -0.0361733
	-0.4607 -0.277468 -0.916762
Cloud after filtering:
	-0.397406 -0.473106 0.292602
	-0.731898 0.667105 0.441304

 但是在我電腦上運行時，得到的點雲值均比較大，重新設置閾值爲0~200後，得到結果如下：

D:\asher\project\FiltersInPCL\bin>FiltersInPCL.exe
Cloud before filtering:
    1.28125 577.094 197.938
    828.125 599.031 491.375
    358.688 917.438 842.563
    764.5 178.281 879.531
    727.531 525.844 311.281
Cloud after filtering:
    1.28125 577.094 197.938

原因是官方例子中的Rand()返回的數值與我的電腦上返回數值不一致，我的返回數值參考http://www.cplusplus.com/reference/cstdlib/rand/可知結果正確的，但是官方例子Rand()返回的爲0到1之間小數，調用的爲excel函數，可參考https://baike.baidu.com/item/rand%28%29。

對原始代碼稍作修改，讀入一ply文件測試結果，

原始點雲：

沒有調用setFilterLimitsNegative(true)時結果：

調用setFilterLimitsNegative(true)後：

可以看到在我的ply文件中有兩個平面，當我設置一個合適的閾值時，就可以得到其中的一個平面，這是其中的一種垂直於座標軸較極端情況。

源碼和具體工程請參考https://github.com/yazhouzheng/FilteringInPCL.git 中的 passthrough.cpp 文件。

（2）VoxelGrid filter

VoxelGrid filter 是通過體素化網格方法對點雲數據集進行降採樣（即減少點雲的個數)。體素網格類在輸入點雲上創建一個三維體素網格。我們可以把每個體素網格想象成空間中一組微小的三維立方體。然後在每個體素中（即每個三維立方體中），所有存在的點都近似其質心（即，向下採樣）。這種方法比用體素的中心來近似要慢一些，但它更準確地表示了點雲所處的潛在曲面。VoxelGrid filter 使用方式如下：

  // Create the filtering object
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloud);
  sor.setLeafSize(10.0f, 10.0f, 10.0f);//leaf size is 10m
  sor.filter (*cloud_filtered);

以上代碼創建了一個leaf size爲10m的 pcl::voxelgrid 過濾器，通過setInputCloud 傳入原始點雲數據，計算後輸出降採樣後的點雲並存儲在cloud_filtered中。

官網示例中讀取的爲PCD文件，由於不易查看，因此在我工程裏改爲讀取ply文件。

當設置leaf size 設置爲1cm時，由於leaf size太小不能採樣，會有以下錯誤

PointCloud before filtering: 16454 data points (x y z).
[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.PointCloud after filtering: 16454 data points (x y z).

 因此，這裏leaf size 設置爲10m,結果如下：

D:\asher\project\FiltersInPCL\bin>FiltersInPCL.exe test.ply
PointCloud before filtering: 16454 data points (x y z).
PointCloud after filtering: 5153 data points (x y z).

 讀入ply文件結果如下，可以明顯的看到點雲減少到原來的1/3左右。

D:\asher\project\FiltersInPCL\bin>FiltersInPCL.exe test.ply
PointCloud before filtering: 16454 data points (x y z).
PointCloud after filtering: 5153 data points (x y z).

具體點雲結果如下，左邊爲原始點雲，右邊爲降採樣後的點雲：

源碼和具體工程請參考https://github.com/yazhouzheng/FilteringInPCL.git 中的 voxel_grid.cpp 文件。

（3）StatisticalOutlierRemoval

StatisticalOutlierRemoval filter是運用統計分析的方法從點雲中去除測量中造成的噪聲,比如說點雲中的一些離羣點。

背景知識

由激光掃描得到的點雲的點密度通常是不確定的，3D結構光解碼出來的每一幀點雲數量也是不一樣的，此外，測量誤差也會有一些稀疏的異常值，這會對結果造成更大的破壞。這使得對局部點雲特徵（如表面法線或曲率變化）的估計變得複雜，從而導致錯誤的值，進而可能導致點雲registration失敗。類似於這樣的部分反常點可以通過對每個點的鄰域進行統計分析來解決，然後移除這些不符合某個標準的點。

StatisticalOutlierRemoval是基於輸入數據集中點到鄰域中點距離的分佈。對於每個點，我們計算從它到所有相鄰點的平均距離。假設結果是有一個平均值和一個標準偏差的高斯分佈，則所有平均距離大於由全局均值和標準偏差距離定義的間隔的點都可以被視爲離羣點（即超過K近鄰平均距離達到某一閾值時，就被認爲是離羣點），並從數據集中去除。

PCL中使用方式如下：

  // Create the filtering object
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud);
  sor.setMeanK (50);
  sor.setStddevMulThresh (1.0);
  sor.filter (*cloud_filtered);

以上代碼創建了pcl::StatisticalOutlierRemoval濾波器對象，setMeanK設置對每一個點所要分析的近鄰點的個數, setStddevMulThresh設置標準差乘數，這裏設置爲1，這意味着，當近鄰點到當前點的均值距離大於一個標準差時，則當前點就會被標記爲離羣點並移除，最終點雲保存在cloud_filtered中。

輸入一ply文件，運行程序結果如下：

D:\asher\project\FiltersInPCL\bin>FiltersInPCL.exe test.ply
Cloud before filtering:
points[]: 16454
width: 16454
height: 1
is_dense: 0
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

Cloud after filtering:
points[]: 15933
width: 15933
height: 1
is_dense: 0
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

通過點雲個數可以看到，我們移除了521個離羣點。

原始點雲：

濾波後點雲：

使用同樣的參數調用濾波器，但是通過設置setNegative傳入true得到負向輸出，

  sor.setNegative (true);
  sor.filter (*cloud_filtered);

則可以得到離羣點的點雲，如下圖所示：

源碼和具體工程請參考https://github.com/yazhouzheng/FilteringInPCL.git 中的 statistical_removal.cpp 文件。

（4）Project points onto a parametric model

ProjectInliers可以把點雲映射到參數化的模型裏面（比如平面，球體等等），參數模型是通過一組係數給出的，比如說可以通過ax + by + cz + d = 0來表示一個平面，則a, b, c, d即可表示一個平面。

ProjectInliers使用方式：

  // Create a set of planar coefficients with X=Y=0,Z=1
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  coefficients->values.resize (4);
  coefficients->values[0] = coefficients->values[1] = 0;
  coefficients->values[2] = 1.0;
  coefficients->values[3] = 0;

  // Create the filtering object
  pcl::ProjectInliers<pcl::PointXYZ> proj;
  proj.setModelType (pcl::SACMODEL_PLANE);
  proj.setInputCloud (cloud);
  proj.setModelCoefficients (coefficients);
  proj.filter (*cloud_projected);

以上代碼首先指定了平面的a = b = d = 0, c=1, 即 z = 0 平面（x, y軸所在的平面），然後創建ProjectInliers對象，並指定z = 0時的模型係數作爲將要映射的平面，最後把結果點雲保存在cloud_projected中。結果如下：

D:\asher\project\FiltersInPCL\bin>FiltersInPCL.exe test.ply
Cloud before projection:
    1.28125 577.094 197.938
    828.125 599.031 491.375
    358.688 917.438 842.563
    764.5   178.281 879.531
    727.531 525.844 311.281
Cloud after projection:
    1.28125 577.094 0
    828.125 599.031 0
    358.688 917.438 0
    764.5   178.281 0
    727.531 525.844 0

原始點雲：

映射到xy平面後結果：

源碼和具體工程請參考https://github.com/yazhouzheng/FilteringInPCL.git 中的 project_inliers.cpp 文件。

（5）Extracting indices from a PointCloud

pcl::ExtractIndices可根據分割算法得到的索引從點雲中提取點的子集，使用方式如下：

1、定義分割對象，並設置參數

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	// Create the segmentation object
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	// Optional
	seg.setOptimizeCoefficients(true);
	// Mandatory
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(1000);
	seg.setDistanceThreshold(10.0);

2、實例化ExtractIndices，並以分割算法得到的點索引從原點雲中提取點雲的子集。爲了處理多個模型，在while循環中運行這個過程，在提取每個模型之後，就返回獲取剩餘的點，再次迭代

	// Create the filtering object
	pcl::ExtractIndices<pcl::PointXYZ> extract;

	int i = 0, nr_points = (int)cloud_filtered->points.size();
	// While 30% of the original cloud is still there
	while (cloud_filtered->points.size() > 0.3 * nr_points)
	{
		// Segment the largest planar component from the remaining cloud
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);

		std::cerr << "Model coefficients: " << coefficients->values[0] << " "
			<< coefficients->values[1] << " "
			<< coefficients->values[2] << " "
			<< coefficients->values[3] << std::endl;

		if (inliers->indices.size() == 0)
		{
			std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
			break;
		}

		// Extract the inliers
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_p);
		std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

		std::stringstream ss;
		ss << "table_scene_lms400_plane_" << i << ".ply";
		pcl::io::savePLYFile(ss.str(), *cloud_p);


		// Create the filtering object
		extract.setNegative(true);
		extract.filter(*cloud_f);
		cloud_filtered.swap(cloud_f);
		i++;
	}

 運行程序，結果如下，可以看到這裏進行了2次迭代，每次迭代所得到的模型係數也不同，最後把原點雲分成了2個平面：

D:\asher\project\FiltersInPCL\bin>FiltersInPCL.exe test.ply
PointCloud before filtering: 16454 data points.
PointCloud after filtering: 5153 data points.
Model coefficients: 0.249878 -0.000696688 0.968277 -672.713
PointCloud representing the planar component: 3520 data points.
Model coefficients: 0.245069 -0.00356539 0.969499 -470.963
PointCloud representing the planar component: 1530 data points.

 原點雲：

第一次迭代：

第二次迭代：

 

 源碼和具體工程請參考https://github.com/yazhouzheng/FilteringInPCL.git 中的 extract_indices.cpp 文件。

（6）Removing outliers using a Conditional or RadiusOutlier removal

此處主要介紹兩種移除離羣點的方式：ConditionalRemoval和RadiusOutlierRemoval，ConditionalRemoval用來刪除點雲中不滿足一個或多個給定條件的所有索引，RadiusOutlierRemoval指在給定半徑範圍內，鄰近點個數少於一定數量的點就會被移除。

ConditionalRemoval的使用方式如下：

// build the condition
pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
        pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 600.0)));
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
        pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 900.0)));

// build the filter
pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
condrem.setCondition(range_cond);
condrem.setInputCloud(cloud);
condrem.setKeepOrganized(true);
// apply filter
condrem.filter(*cloud_filtered);

首先需要通過pcl::ConditionAnd<pcl::PointXYZ>設定濾波的條件，在上面的代碼裏添加了2個比較的條件 Z>600 && Z<900, 然後用此條件構造ConditionalRemoval濾波器，最後濾波後的點雲存儲在cloud_filtered中。

RadiusOutlierRemoval背景知識

以下圖片有助於RadiusOutlierRemoval的理解，用戶來指定點雲中每個點在特定半徑範圍內必須有的鄰近點個數，如果指定鄰近點個數爲1則灰色的點會被移除，假定指定鄰近點個數爲3則灰色和紅色的點均會被移除

RadiusOutlierRemoval的使用方式如下：

pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
// build the filter
outrem.setInputCloud(cloud);
outrem.setRadiusSearch(50.0);
outrem.setMinNeighborsInRadius(50);
// apply filter
outrem.filter(*cloud_filtered);

 以上代碼中實例化了一個RadiusOutlierRemoval對象，setRadiusSearch設置鄰域搜索半徑，setMinNeighborsInRadius設置鄰近點個數。

原始點雲：

ConditionalRemoval濾波後結果：

RadiusOutlierRemoval濾波後結果：

 源碼和具體工程請參考https://github.com/yazhouzheng/FilteringInPCL.git 中的 remove_outliers.cpp 文件。

3 Filters 總結

本文介紹了PCL官方文檔中的六種基本的Filter，分別是高/低通(passthrough), 降採樣(voxelgrid), 基於統計學的離羣點移除(statistical-outlier-removel), 映射點雲到平面或曲面(project-inliers), 分割並提取點雲(extract-indices), 基於條件或半徑移除點雲(remove-outliers), 這些基本的Filter主要用於點雲的預處理，然後爲之後的點雲算法提供更加規範的輸入，本文所有示例代碼均可可以順利編譯運行且可以在https://github.com/yazhouzheng/FilteringInPCL找到。

4 Reference:

1. http://www.pointclouds.org/documentation/tutorials/passthrough.php#passthrough

2. http://www.pointclouds.org/documentation/tutorials/voxel_grid.php#voxelgrid

3. http://www.pointclouds.org/documentation/tutorials/statistical_outlier.php#statistical-outlier-removal

4. http://www.pointclouds.org/documentation/tutorials/project_inliers.php#project-inliers

5. http://www.pointclouds.org/documentation/tutorials/extract_indices.php#extract-indices

6. http://www.pointclouds.org/documentation/tutorials/remove_outliers.php#remove-outliers