kd_tree 半径搜索

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/impl/io.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/common/time.h>

#include <pcl/filters/radius_outlier_removal.h>//半径滤波器
#include <boost/make_shared.hpp>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/common/pca.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <pcl/features/moment_of_inertia_estimation.h>
using namespace std;
void spherical_search(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointXYZ search_point, double spherical_radius, pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud_spherical)
{
	//函数说明：找到待搜索点球形邻域内的邻域点
	//参数说明:  cloud全部点云            search_point 待搜索点          spherical_radius 球形邻域的半径    cloud_spherical邻域点
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; //创建KdTreeFLANN 对象 注意<>这里的点云类型要保持一致
	kdtree.setInputCloud(cloud);//添加点云
	std::vector<int> pointIdxRadiusSearch;
	std::vector<float> pointRadiusSquaredDistance;//邻域点的待搜索点距离的平方
	if (kdtree.radiusSearch(search_point, spherical_radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
	{
		cout<<pointIdxRadiusSearch.size()<<"size"<<endl;
    for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
		{
			if(pointRadiusSquaredDistance[i]*2>0.48)
			{

			cloud_spherical->push_back(cloud->points[pointIdxRadiusSearch[i]]);
     		 cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x
				<< " " << cloud->points[pointIdxRadiusSearch[i]].y
				<< " " << cloud->points[pointIdxRadiusSearch[i]].z
				<< " (squared distance: " << pointRadiusSquaredDistance[i]*2<< ")" << endl;

			}
		}
	}

}

int main(int argc, char* argv[])
{
	//点云读取+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	// pcl::io::loadPCDFile<pcl::PointXYZ>("kd_tree.pcd", *cloud);
    // if (pcl::io::loadPCDFile(argv[1], *cloud) < 0)
    // {
    //     PCL_ERROR("Could not read file\n");
    //     return (-1);
    // }

    float side_length = 1.0;

    // 在正方形内生成点云
    for (float x = -side_length / 2; x <= side_length / 2; x += 0.01) {
        for (float y = -side_length / 2; y <= side_length / 2; y += 0.01) {
            pcl::PointXYZ point;
            point.x = x;
            point.y = y;
            point.z = 0.0;  // 在平面上，z坐标可以设置为任意值
            cloud->points.push_back(point);
        }
    }


	//搜索某点的球形邻域点++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//假设现在要找到某个点如cloud[5]的半径0.5米的球形邻域中的点，保存至cloud_spherical容器并且显示  //当然了，这里某个点的序号肯定是小于所有点云总个数的哈哈
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_spherical(new pcl::PointCloud<pcl::PointXYZ>);
	double spherical_radius =0.5;
  pcl::PointXYZ serach_point;
  serach_point.x=0;
  serach_point.y=0;
  serach_point.z=0;
  	cout << "Neighbors within radius search at (" << serach_point.x
		<< " " << serach_point.y
		<< " " << serach_point.z
		<< ") with radius=" << spherical_radius << endl;
	cout << endl;

	spherical_search(cloud, serach_point, spherical_radius, cloud_spherical);


















	//显示++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); //创建视窗对象，定义标题栏名称“3D Viewer”


	viewer->addPointCloud<pcl::PointXYZ>(cloud, "original_cloud");	//将点云添加到视窗对象中，并定义对应的ID“original_cloud”
	//用于改变显示点云的尺寸，可以利用该方法控制点云在视窗中的显示方法,1设置显示点云大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");//PCL_VISUALIZER_POINT_SIZE点大小设置，
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1,1,1, "original_cloud"); //PCL_VISUALIZER_COLOR颜色设置 RGB
	

	viewer->addPointCloud<pcl::PointXYZ>(cloud_spherical, "spherical_cloud");	//将点云添加到视窗对象中，并定义对应的ID“original_cloud”
	//用于改变显示点云的尺寸，可以利用该方法控制点云在视窗中的显示方法,1设置显示点云大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "spherical_cloud");//PCL_VISUALIZER_POINT_SIZE点大小设置，
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0.5, "spherical_cloud"); //PCL_VISUALIZER_COLOR颜色设置 RGB
	viewer->addCoordinateSystem(1.0);
	//防止显示窗口一闪而过
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}





	system("pause");
	return 0;
}