K-d tree and octree of PCL

utilize k-d tree Realize fast neighborhood search

#include <iostream>
#include <vector> // The dynamic array vector The header file 
#include <ctime> // System time header file 
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h> //k Nearest neighbor search header file 

int main(int argc, char** argv)
{
    
	srand(time(NULL));																		// System random number seed 

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);			// Initialize a point cloud with random numbers 
	cloud->width = 1000;
	cloud->height = 1;
	cloud->points.resize(cloud->width * cloud->height);
	for (size_t i = 0; i < cloud->points.size(); ++i)
	{
    
		cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
	}

	pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;													// establish kd tree
	kdtree.setInputCloud(cloud);															// Set up kd tree  Search scope for 
	pcl::PointXYZ searchpoint;																// Set the search center , And assign a random value to the store 
	searchpoint.x = 1024 * rand() / (RAND_MAX + 1.0f);
	searchpoint.y = 1024 * rand() / (RAND_MAX + 1.0f);
	searchpoint.z = 1024 * rand() / (RAND_MAX + 1.0f);

	int K = 10;																				// Set search k The number of nearest neighbors is 10
	std::vector<int>pointIdxNKNSearch(K);													// Set the index of the nearest neighbor of the query point 
	std::vector<float>pointNKNSquareDistance(K);											// Store the square distance corresponding to the nearest neighbor 
	std::cout << "K nearest neightbor searchpoint at (" << searchpoint.x << " " << searchpoint.y << " " << searchpoint.z << ") with K = " << K << std::endl;

	if (kdtree.nearestKSearch(searchpoint, K, pointIdxNKNSearch, pointNKNSquareDistance) > 0)// If kd tree If there is a nearest neighbor, output , If it does not exist, it will not output 
	{
    
		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:" << pointNKNSquareDistance[i] << ")" << std::endl;
		}
	}
	else
	{
    
		std::cout << "NO K nearested point detected" << std::endl;
	}

	std::cout << "\n" << std::endl;

	std::vector<int>pointIdxRadiusSearch;													// Set to search the nearest neighbor within the radius 
	std::vector<float>pointRadiusSquareDistance;											// The square distance corresponding to the nearest neighbor in the radius 
	float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
	std::cout << "Neighbors within radius search at(" << searchpoint.x << " " << searchpoint.y << " " << searchpoint.z << ") WIth Radius=" << radius << std::endl;

	if (kdtree.radiusSearch(searchpoint, radius, pointIdxRadiusSearch, pointRadiusSquareDistance) > 0)// Output the searched nearest neighbor and the corresponding radius information 
	{
    
		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:" << pointRadiusSquareDistance[i] << ")" << std::endl;
		}
	}
	else
	{
    
		std::cout << "No enough points detected in Radius Search!" << std::endl;
	}
	return(0);

}

Use octree to search

#include<pcl/point_cloud.h>
#include<pcl/octree/octree_search.h>

#include<iostream>
#include<vector>
#include<ctime>

using namespace std;

int main(int argc, char** argv)
{
    
	// Use system time to seed random numbers 
	srand((unsigned int)time(NULL));
	// Create a PointXYZ Type point cloud pointer 
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	// Initialize point cloud data 
	cloud->width = 1000;// Wide for 1000
	cloud->height = 1;// High for 1, Description is disordered point cloud 
	cloud->points.resize(cloud->width * cloud->height);
	// Fill the data with random numbers 
	for (size_t i = 0; i < cloud->size(); ++i)
	{
    
		//PointCloud Class [] Operator is overloaded , The return is right points References to 
		// (*cloud)[i].x  Equate to  cloud->points[i].x
		(*cloud)[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);// Progressive writing 
		cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);// Progressive writing 
	}

	// Create an octree object 
	float resolution = 128.0f;// Set resolution 
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);


	// Set point cloud input , Will be in cloud Mid search 
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	// Set the searched point , Fill with random numbers 
	pcl::PointXYZ searchPoint;
	searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
	searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
	searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

	// Voxel inner nearest neighbor search 
	// Use vector Store search result subscripts 
	vector<int> pointIdxVec;// Save subscript 
	if (octree.voxelSearch(searchPoint, pointIdxVec))
	{
    
		cout << "Neighbors within voxel search at (" << searchPoint.x
			<< " " << searchPoint.y
			<< " " << searchPoint.z
			<< ")"
			<< endl;
		for (size_t i = 0; i < pointIdxVec.size(); ++i)
		{
    
			cout << " " << cloud->points[pointIdxVec[i]].x
				<< " " << cloud->points[pointIdxVec[i]].x
				<< " " << cloud->points[pointIdxVec[i]].z
				<< endl;
		}
	}

	// Start k Nearest neighbor search 
	int K = 10;
	// Use two vector Store search results 
	vector<int> pointIdxNKNSearch(K);// Save subscript 
	vector<float> pointNKNSquaredDistance(K);// Save the square of the distance 

	cout << "K nearest neighbor search at (" << searchPoint.x
		<< " " << searchPoint.y
		<< " " << searchPoint.z
		<< ") with K = " << K << endl;



	/** *  Suppose our KdTree Return more than 0 The nearest neighbor , *  Then it prints out all 10 Random separation searchPoint The location of the nearest neighbor , *  These are stored in what we created before vector in . */
	if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	{
    
		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
		{
    
			cout << " " << cloud->points[pointIdxNKNSearch[i]].x
				<< " " << cloud->points[pointIdxNKNSearch[i]].x
				<< " " << cloud->points[pointIdxNKNSearch[i]].z
				<< "( squared distance: " << pointNKNSquaredDistance[i] << " )" << endl;
		}
	}

	// Radius based neighborhood search 
	// The search results are saved in two arrays , One is the subscript , One is distance 
	vector<int> pointIdxRadiusSearch;
	vector<float> pointRadiusSquaredDistance;

	// Set the search radius , Random value 
	float radius = 256.0f* rand() / (RAND_MAX + 1.0f);

	cout << "Neighbors within radius search at (" << searchPoint.x
		<< " " << searchPoint.y
		<< " " << searchPoint.z
		<< ") with radius=" << radius << endl;

	/** *  If our KdTree Returns more than... Within the specified radius 0 A neighbor , It will print out the coordinates of these points stored in the vector . */
	if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
	{
    
		for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
		{
    
			cout << " " << cloud->points[pointIdxRadiusSearch[i]].x
				<< " " << cloud->points[pointIdxRadiusSearch[i]].x
				<< " " << cloud->points[pointIdxRadiusSearch[i]].z
				<< "( squared distance: " << pointRadiusSquaredDistance[i] << " )" << endl;
		}
	}

	return 0;
}