PCL利用kdtree搜寻邻域点代码

PCL利用kdtree搜寻邻域点代码

• 代码功能：用两种方式搜索并且显示某点的邻域点的坐标和距离信息（半径限制和个数限制）
• 代码缺陷：没有添加可视化过程

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/io/io.h>  
#include <pcl/io/pcd_io.h>  
#include <pcl/point_types.h>  
#include <pcl/visualization/cloud_viewer.h>  
using namespace std;
using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;

int main(int argc, char** argv)
{
	std::string filename("C:\\Users\\426-4\\Desktop\\2_gt.pcd");
	//visualizer
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::io::loadPCDFile(filename, *cloud);
	//搜索source关键点周围的点云
	//创建kdtree 结构
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	//传入点云
	//source_Ransac_keypoint_trans
	kdtree.setInputCloud(cloud);
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_key_Neigh(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_key_point(new pcl::PointCloud<pcl::PointXYZ>());
	std::vector<int> pointIdxRadiusSearch;
	std::vector<float> pointRadiusSquaredDistance;
	std::vector<int> pointIdxNKNSearch;
	std::vector<float> pointNKNSquaredDistance;
	//设置输入点
	pcl::PointXYZ searchPoint;    //source_22
	int j;
	for (int i = 0; i < cloud->size(); i++)

	{
		//设置输入点条件
		if (i == 10)
		{
			searchPoint = cloud->at(i);
			source_key_point->push_back(cloud->at(i));
			//设置邻域半径
			float radius = 0.005;
			int K = 10;
			//利用最小半径搜索邻近点
			//if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
			//{
			//	for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
			//		std::cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x
			//		<< " " << cloud->points[pointIdxRadiusSearch[i]].y
			//		<< " " << cloud->points[pointIdxRadiusSearch[i]].z
			//		<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
			//}
			
			//利用最少点数搜索邻近点
			if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
			{
				for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
					std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x
					<< " " << cloud->points[pointIdxNKNSearch[i]].y
					<< " " << cloud->points[pointIdxNKNSearch[i]].z
					<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
			}
			break;
		}

	}

}

结果实例：

    0.07308 -0.00916 0.00951 (squared distance: 0)
    0.07357 -0.0144 0.0197 (squared distance: 0.000131534)
    0.07563 -0.00802 -0.00166 (squared distance: 0.000132571)
    0.08422 -0.00824 0.00407 (squared distance: 0.00015454)
    0.06549 -0.00812 -0.00243 (squared distance: 0.000201253)
    0.084 -0.01291 0.0178 (squared distance: 0.000202033)
    0.05903 -0.01183 0.01584 (squared distance: 0.0002446)
    0.08882 -0.01174 0.01563 (squared distance: 0.000291858)
    0.05964 -0.00865 -0.00642 (squared distance: 0.000434659)
    0.0584 -0.01665 0.02257 (squared distance: 0.000442166)