简介

八叉树是一种用于管理稀疏3D点云的树状数据结构，每个内部节点都正好有八个子节点。可实现“体素内近邻搜索”，“K近邻搜索”，“半径内近邻搜索”

代码实现

#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>

using namespace std;
int
main(int argc, char**argv)
{
	srand((unsigned int)time(NULL));
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	// 创建点云数据
	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.0f* rand() / (RAND_MAX + 1.0f);
		cloud->points[i].y = 1024.0f* rand() / (RAND_MAX + 1.0f);
		cloud->points[i].z = 1024.0f* rand() / (RAND_MAX + 1.0f);
	}
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	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);
	// 体素内近邻搜索
	vector<int>pointIdxVec;
	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)
			std::cout << "    " << cloud->points[pointIdxVec[i]].x
			<< " " << cloud->points[pointIdxVec[i]].y
			<< " " << cloud->points[pointIdxVec[i]].z << std::endl;
	}
	//K近邻搜索
	int K = 10;
	vector<int>pointIdxNKNSearch;
	vector<float>pointNKNSquaredDistance;
	cout << "K nearest neighbor search at (" << searchPoint.x
		<< " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << endl;
	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]].y
			<< " " << cloud->points[pointIdxNKNSearch[i]].z
			<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << endl;
	}
	//半径内近邻搜索
	     vector<int>pointIdxRadiusSearch;
	     vector<float>pointRadiusSquaredDistance;
	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 (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]].y
			<< " " << cloud->points[pointIdxRadiusSearch[i]].z
			<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << endl;
	}
	return 0;
        }