PCL 體速柵格下采樣

體素柵格下采樣方法，通過設定小立方體的size，計算它的重心（而不是中心）代替其中所有的點，進行下采樣 

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

#include "stdafx.h"

//體素柵格下采樣zxr
//int _tmain(int argc, _TCHAR* argv[])
//{
//	return 0;
//}
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/search.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <sstream>

using namespace std;
using namespace pcl;

int
main(int argc, char** argv)
{
	PointCloud<PointNormal>::Ptr cloud(new PointCloud<PointNormal>());//PointCloud 2進制儲存的點雲數據
	PointCloud<PointNormal>::Ptr cloud_filtered(new PointCloud<PointNormal>());
	PointCloud<PointXYZ> ::Ptr pCloud(new PointCloud<PointXYZ>);
	PointCloud<PointXYZ> ::Ptr filtered_pCloud(new PointCloud<PointXYZ>);

	string inputFile= "bunny.ply";
	io::loadPLYFile(inputFile, *cloud);
	io::loadPLYFile(inputFile, *pCloud);

	std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
		<< " data points (" << pcl::getFieldsList(*cloud) << ").";

	// Create the filtering object
	pcl::VoxelGrid<PointXYZ> sor;
	sor.setInputCloud(pCloud);
	sor.setLeafSize(0.003f, 0.003f, 0.003f);//設置柵格體素的大小  重要！！！
	sor.filter(*filtered_pCloud);

	//根據下采樣的結果，選擇距離採樣點最近的點作爲特徵點，返回特徵點的索引
	KdTreeFLANN<PointXYZ> kdt;
	kdt.setInputCloud(pCloud);
	PointIndicesPtr inds = boost::shared_ptr<PointIndices>(new PointIndices());//記錄採樣後根據最鄰近點提取的樣本點下標索引，也是所謂的關鍵點索引
	for (size_t i = 0; i < filtered_pCloud->points.size(); i++) {
		PointXYZ p0;
		p0.x = filtered_pCloud->points[i].x; p0.y = filtered_pCloud->points[i].y; p0.z = filtered_pCloud->points[i].z;
		vector<int> temp_id(1);
		vector<float> dis(1);
		if (kdt.nearestKSearch(p0, 1, temp_id, dis)>0){
			inds->indices.push_back(temp_id[0]);
		}
	}
	copyPointCloud(*cloud, inds->indices, *cloud_filtered);
	cout << "the number of downsampling cloud2 " << cloud_filtered->points.size() << endl;
	PLYWriter plyw;
	plyw.write("downsamed.ply", *cloud_filtered);
	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
		<< " data points (" << pcl::getFieldsList(*cloud_filtered) << ").";

	pcl::PCDWriter writer;
	stringstream ss;
	ss << "downsample_" << inputFile;
	io::savePLYFile(ss.str(), *cloud_filtered);

	return (0);
}