最近邻点提取

#include <iostream>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <boost/thread/thread.hpp>
#include <pcl/kdtree/kdtree_flann.h>  //kdtree近邻搜索
using namespace std;

int main() {
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>("a.pcd", *cloud);
	//定义查询点,点类型是PointXYZ
	pcl::PointCloud<pcl::PointXYZ>::Ptr searchCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>("centerPt2.pcd", *searchCloud);

	//定义提取点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr slice(new pcl::PointCloud<pcl::PointXYZ>);
	std::vector<int> finalPoint;

	// k近邻搜索
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;	//创建k-d tree对象
	kdtree.setInputCloud(cloud);			//设置搜索空间
	int K = 1;
	std::vector<int> pointIdxNKNSearch(K);			//存储查询点近邻索引  Vector<类型>标识符(最大容量)
	std::vector<float> pointNKNSquaredDistance(K);	//存储近邻点对应平方距离
	//std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl;

	for (size_t i = 0; i < searchCloud->points.size(); i = i++) {

		pcl::PointXYZ searchPoint = searchCloud->points[i];

		//假设kdtree对象返回了多于0个近邻，搜索结果已经存储在pointIdxNKNSearch和pointNKNSquaredDistance向量中。
		if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
		{
			for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i) {		//打印出所有近邻坐标
				std::cout << "pointIdxNKNSearch["<< i<<"]" << pointIdxNKNSearch[i]
					<< "    " << cloud->points[pointIdxNKNSearch[i]].x
					<< " " << cloud->points[pointIdxNKNSearch[i]].y
					<< " " << cloud->points[pointIdxNKNSearch[i]].z
					//<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" 
					<< std::endl;
				finalPoint.push_back(pointIdxNKNSearch[i]);
				
			}
		}
	}

	slice->width = finalPoint.size();
	slice->height = 1;
	slice->is_dense = false;
	slice->points.resize(slice->width * slice->height);
	for (size_t p = 0; p < finalPoint.size(); ++p) {
		//cout << "finalPoint " << finalPoint[p] << endl;
		slice->points[p].x = cloud->points[finalPoint[p]].x;
		slice->points[p].y = cloud->points[finalPoint[p]].y;
		slice->points[p].z = cloud->points[finalPoint[p]].z;
	}
	pcl::io::savePCDFileASCII("aaaaaa.pcd", *slice);

	return 0;
}