PCL使用教程-使用八叉树（Octrees）进行空间分割和搜索操作

原文链接：Spatial Partitioning and Search Operations with Octrees — Point Cloud Library 0.0 documentation

使用八叉树进行空间划分和搜索操作

八叉树是用于管理稀疏3-D数据的基于树的数据结构。 每个内部节点都有八个子节点。 在本教程中，我们将学习如何使用八叉树在点云数据中进行空间分区和邻域搜索。 特别地，我们说明如何执行“体素内近邻搜索（Neighbors within Voxel Search）”，“ K最近邻搜索（K Nearest Neighbor Search）”和“半径内近邻搜索（Neighbors within Radius 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)
{
	//使用系统时间做随机数种子
	srand((unsigned int)time(NULL));
	//创建一个PointXYZ类型点云指针
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	//初始化点云数据
	cloud->width = 1000;//宽为1000
	cloud->height = 1;//高为1，说明为无序点云
	cloud->points.resize(cloud->width * cloud->height);
	//使用随机数填充数据
	for (size_t i = 0; i < cloud->size(); ++i)
	{
		//PointCloud类中对[]操作符进行了重载，返回的是对points的引用
		// (*cloud)[i].x 等同于 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);//推进写法
		cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);//推进写法
	}

	//创建八叉树对象
	float resolution = 128.0f;//设置分辨率
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);


	//设置点云输入,将在cloud中搜索
	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存储搜索结果下标
	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)
		{
			cout << "    " << cloud->points[pointIdxVec[i]].x
				<< " " << cloud->points[pointIdxVec[i]].x
				<< " " << cloud->points[pointIdxVec[i]].z
				<< endl;
		}
	}

	//开始k最近邻搜索
	int K = 10;
	//使用两个vector存储搜索结果
	vector<int> pointIdxNKNSearch(K);//保存下标
	vector<float> pointNKNSquaredDistance(K);//保存距离的平方

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



	/**
	 * 假设我们的KdTree返回超过0个最近的邻居，
	 * 然后它打印出所有10个离随机searchPoint最近的邻居的位置，
	 * 这些都存储在我们之前创建的vector中。
	 */
	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;
		}
	}

	//基于半径的邻域搜索
	//搜索结果保存在两个数组中，一个是下标，一个是距离
	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;

	/**
	 * 如果我们的KdTree在指定的半径内返回超过0个邻居，它将打印出这些存储在向量中的点的坐标。
	 */
	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;
}

我们创建一个octree实例，并使用其分辨率对其进行初始化。 此八叉树在其叶节点内保留一个点索引向量。 分辨率参数描述了最低八叉树级别上最小体素的长度。 因此，八叉树的深度是分辨率以及点云的空间尺寸的函数。 如果知道pointcloud的边界框，则应使用defineBoundingBox方法将其分配给八叉树。 然后，我们为PointCloud分配一个指针，并将所有点添加到八叉树。

一旦PointCloud与八叉树相关联，我们就可以执行搜索操作。 此处使用的第一搜索方法是“体素搜索中的邻居”。 它将搜索点分配给相应的叶节点体素，并返回点索引的向量。 这些索引与属于同一体素的点有关。 搜索点和搜索结果之间的距离因此取决于八叉树的分辨率参数。

运行结果：

Neighbors within voxel search at (806.75 753.406 597.719)
    879.125 879.125 576.156
K nearest neighbor search at (806.75 753.406 597.719) with K = 10
    763.156 763.156 562.438( squared distance: 5947.56 )
    783.281 783.281 516.313( squared distance: 7265.65 )
    828.875 828.875 605.344( squared distance: 8002.9 )
    879.125 879.125 576.156( squared distance: 10050.8 )
    836.688 836.688 625.438( squared distance: 10140.1 )
    752.938 752.938 627.063( squared distance: 14729.4 )
    687.281 687.281 537.563( squared distance: 18527.5 )
    896.781 896.781 499.719( squared distance: 19473.6 )
    831.531 831.531 587.344( squared distance: 22487.2 )
    705.094 705.094 578.063( squared distance: 23250.4 )
Neighbors within radius search at (806.75 753.406 597.719) with radius=232.852
    855.125 855.125 478.844( squared distance: 53407.4 )
    874.375 874.375 484.313( squared distance: 37191.9 )
    710.969 710.969 460.906( squared distance: 29724.6 )
    791.844 791.844 473.781( squared distance: 23470.4 )
    794.594 794.594 410.188( squared distance: 47986.1 )
    796.75 796.75 450.188( squared distance: 27896 )
    919.875 919.875 491.375( squared distance: 34818.5 )
    896.781 896.781 499.719( squared distance: 19473.6 )
    641.844 641.844 648.906( squared distance: 38874.9 )
    656.969 656.969 704.313( squared distance: 33799 )
    719.531 719.531 640.375( squared distance: 29070.4 )
    808.594 808.594 503( squared distance: 53430.1 )
    831.531 831.531 587.344( squared distance: 22487.2 )
    783.281 783.281 516.313( squared distance: 7265.65 )
    705.094 705.094 578.063( squared distance: 23250.4 )
    687.281 687.281 537.563( squared distance: 18527.5 )
    763.156 763.156 562.438( squared distance: 5947.56 )
    716.813 716.813 710.5( squared distance: 24896.4 )
    752.938 752.938 627.063( squared distance: 14729.4 )
    791.813 791.813 727.5( squared distance: 31466.3 )
    836.688 836.688 625.438( squared distance: 10140.1 )
    828.875 828.875 605.344( squared distance: 8002.9 )
    879.125 879.125 576.156( squared distance: 10050.8 )
    722.781 722.781 760.094( squared distance: 39485.8 )
    747.844 747.844 777( squared distance: 41330.9 )
    850.156 850.156 800.625( squared distance: 51324.7 )
    907.844 907.844 756.875( squared distance: 36966.3 )
    831.969 831.969 761.563( squared distance: 28520.8 )
    1004.97 1004.97 518.938( squared distance: 47403 )
    999.438 999.438 611( squared distance: 43369.4 )
    947.594 947.594 677.156( squared distance: 26310.6 )
    995.156 995.156 680.469( squared distance: 44020.4 )
    965.656 965.656 642.438( squared distance: 38969 )
    1001.09 1001.09 500.719( squared distance: 49065.3 )
    982.438 982.438 668.438( squared distance: 49388.6 )
    736.25 736.25 494.875( squared distance: 37729.5 )
    721.281 721.281 579.906( squared distance: 31299.7 )
    756.375 756.375 636.781( squared distance: 45006.5 )
    872.25 872.25 589.906( squared distance: 30071.4 )
    879.063 879.063 698.219( squared distance: 44591.7 )
    878.469 878.469 686.969( squared distance: 36470.4 )
    828.031 828.031 701.844( squared distance: 27686.9 )

额外细节

PCL八叉树组件提供了几种八叉树类型。 它们的基本区别在于各自的叶子节点特征。

• OctreePointCloudPointVector（等于OctreePointCloud）：此八叉树可以在每个叶节点处保存点索引列表。
• OctreePointCloudSinglePoint：此八叉树类在每个叶节点上仅保留一个点索引。 仅存储分配给叶节点的最新点索引。
• OctreePointCloudOccupancy：此八叉树在其叶节点上不存储任何点信息。 它可以用于空间占用检查。
• OctreePointCloudDensity：此八叉树计算每个叶节点体素内的点数。 它允许空间密度查询。

如果需要高频率创建八叉树，请查看八叉树双缓冲实现（Octree2BufBase类）。 此类同时在内存中保留两个并行的八叉树结构。 除了搜索操作之外，这还可以进行空间变化检测。 此外，高级内存管理可在八叉树构建过程中减少内存分配和释放操作。 可以通过模板参数“ OctreeT”将双缓冲八叉树实现分配给所有OctreePointCloud类。

所有八叉树都支持八叉树结构和八叉树数据内容的序列化和反序列化。