PCL之ExtractIndices通過分割算法提取部分點雲數據子集

 ExtractIndices通過分割算法提取部分點雲數據子集的下標索引，代碼過程步驟：
 使用之前的體素柵格下采樣方法進行下采樣；
 SAC平面參數模型提取符合該幾何模型的點雲數據子集，再利用分割算法進行提取符合幾何平面的點雲數據子集；
利用negative變量可以提取相反的點雲集剩餘點雲；
利用剩餘點雲作爲待處理的點雲，返回步驟2，進入下一輪循環，直到滿足終止條件

// extractIndices.cpp : 定義控制檯應用程序的入口點。
//

#include "stdafx.h"


//int _tmain(int argc, _TCHAR* argv[])
//{
//	return 0;
//}

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
using namespace std;

int
main(int argc, char** argv)
{
	pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2), cloud_filtered_blob(new pcl::PCLPointCloud2);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>), cloud(new pcl::PointCloud<pcl::PointXYZ>),cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);

	// Fill in the cloud data 讀入數據
	pcl::PCDReader reader;
	reader.read("table_scene_lms400.pcd", *cloud_blob);//二進制點雲數據

	std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;

	// Create the filtering object: downsample the dataset using a leaf size of 1cm phase one 使用1cm的體素柵格（VoxelGrip），進行下采樣濾波
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud(cloud_blob);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);
	sor.filter(*cloud_filtered_blob);

	// Convert to the templated PointCloud
	pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);//二進制點雲數據轉化爲標準點雲數據 
	pcl::fromPCLPointCloud2(*cloud_blob, *cloud);

	std::cerr << "PointCloud after (VoxelGridDownsampling filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;

	// Write the downsampled version to disk
	pcl::PCDWriter writer;
	writer.write<pcl::PointXYZ>("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
	pcl::PLYWriter writer1;
	writer1.write("VoxelGridDownsampling1cm_table.ply", *cloud_filtered);
	writer1.write("origin_table.ply", *cloud);

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());//創建參數模型的的參數對象
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());//儲存模型內點的下標索引的數組指針
	// Create the segmentation object
	pcl::SACSegmentation<pcl::PointXYZ> seg;//創建SAC分割 對象
	// Optional 可選的設置項
	seg.setOptimizeCoefficients(true);//優化參數設置 作用？？？？
	// Mandatory  強制設置選項
	seg.setModelType(pcl::SACMODEL_PLANE);//SAC幾何模型設置選擇plane平面模型
	seg.setMethodType(pcl::SAC_RANSAC);//SAC方法選擇，選擇隨機一致性採樣方法
	seg.setMaxIterations(1000);//最大迭代次數
	seg.setDistanceThreshold(0.01);//設置距離閾值

	// Create the filtering object
	pcl::ExtractIndices<pcl::PointXYZ> extract;//創建提取索引方法的對象
	//phase two 
	int i = 0, nr_points = (int)cloud_filtered->points.size();
	cout << "nr_points  " << nr_points << endl;
	// While 30% of the original cloud is still there  下面過程執行的條件是經過體素柵格下采樣之後必須仍有超過30%的點數據存在
	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);//傳入的參數未進行賦值  利用參數模型進行分割
		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);//在下采樣基礎之上進行分割和ExtractInliers濾波的  結果就是一個平面模型映射的結果
		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_Extract_" << i << ".ply";
		writer1.write<pcl::PointXYZ>(ss.str(), *cloud_p, false);
		//ss.clear();


		// Create the filtering object
		extract.setNegative(true);
		extract.filter(*cloud_f);//這裏提取的是不符合條件的點
		//std::stringstream ss1; 
		//ss1<< "table_scene_lms400_plane_Extract_Neg_" << i << ".ply";
		//writer1.write<pcl::PointXYZ>(ss1.str(), *cloud_f, false);
		////ss.clear();
		cloud_filtered.swap(cloud_f);//在體素柵格下采樣基礎之上得到的點雲數據和提取出的不符合條件的點雲數據進行調換，進入下一輪循環
		//std::stringstream ss2; 
		//ss2 << "table_scene_lms400_plane_down_swap_Extract_Neg_" << i << ".ply";
		//writer1.write<pcl::PointXYZ>(ss2.str(), *cloud_filtered, false);
		////ss.clear();
		i++;
		cout << "cloud_filtered->points.size" << cloud_filtered->points.size() << endl;
	}


	return (0);
}