PCL学习笔记（九）-- 基于八叉树的“体素近邻搜索”、“K近邻搜索”和“半径内近邻搜索”

一、简介

  八叉树可以用于管理稀疏3D数据的树状数据结构，每个内部节点都正好有八个子节点，本小节将介绍如何用八叉树在点云数据中进行空间划分及近邻搜索，特别地，解释了如何完成“体素内近邻搜索”、“K近邻搜索”和“半径内近邻搜索”。

二、代码分析

  1）首先定义并实例化一个PointCloud指针对象，并用随机的点集赋值给它：

srand ((unsigned int) time (NULL));
//首先定义并实例化一个PointCloud指针对象，并用随机点集赋值给它
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);
  }

  2）然后创建一个八叉树实例，用设置分辨率进行初始化，该八叉树用它的叶节点存放点索引向量，该分辨率参数描述最低一级八叉树的最小体素的尺寸，因此八叉树的深度是分辨率和点云空间维数的函数，如果知道点云的边界框，应该用defineBoundingBox方式把它分配给八叉树，然后通过点云指针把所有点增加到八叉树中：

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);

  3）一旦PointCloud和八叉树联系在一起，我们就能进行搜索操作，这里使用的第一种搜索方法是“体素近邻搜索”，他把查询点所在的体素中其他点的索引作为查询结果返回，结果以点索引的向量形式保存，因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数：

std::vector<int>pointIdxVec;
if (octree.voxelSearch (searchPoint, pointIdxVec))
  {
std::cout<<"Neighbors within voxel search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z<<")"
<<std::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;
  }

  4）接下来，介绍k近邻搜索，本例中，k被设置为10，“k近邻搜索”方法把搜索结果写到两个分开保存的向量中，第一个pointIdxNKNSearch包含搜索结果（结果点的索引的向量），第二个向量保存相应的搜索点和近邻之间的平方距离：

int K =10;
std::vector<int>pointIdxNKNSearch;
std::vector<float>pointNKNSquaredDistance;
std::cout<<"K nearest neighbor search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z
<<") with K="<< K <<std::endl;
if (octree.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;
  }

  5）“半径内近邻搜索”原理和“k近邻搜索”类似，它的搜索结果被写入两个分开的向量中，这两个向量分别存储结果点的索引和对应的平方距离：

std::vector<int>pointIdxRadiusSearch;
std::vector<float>pointRadiusSquaredDistance;
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 (octree.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;
  }

  6）整体代码如下：

#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/octree/octree.h>																		//八叉树的头文件

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 * 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);
	}

	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);

	std::vector<int>pointIdxVec;																	//进行体素近邻搜索，将查询点所在的体素中其他的点的索引作为查询结果返回
	if (octree.voxelSearch(searchPoint, pointIdxVec))												//结果以索引的向量形式保存
	{
		std::cout << "Neighbors within voxel search at(" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ")" << std::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;
		}
	}
	else
	{
		std::cout << "Voxel search failed!" << std::endl;
	}

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

	int K = 10;																						//执行k近邻搜索
	std::vector<int>pointIdxNKNSearch(K);															//保存点的索引
	std::vector<float>pointNKNSquareDistance(K);													//保存搜索到达的点与近邻之间的平方距离
	std::cout << "K nearest neightbor searchpoint at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K = " << K << std::endl;

	if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquareDistance) > 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:" << pointNKNSquareDistance[i] << ")" << std::endl;
		}
	}
	else
	{
		std::cout << "NO K nearested point detected" << std::endl;
	}

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

	std::vector<int>pointIdxRadiusSearch;															//执行半径搜索，保存搜索到的点的索引
	std::vector<float>pointRadiusSquareDistance;													//保存搜索到的点与查询点之间的平方距离
	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 (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquareDistance) > 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:" << pointRadiusSquareDistance[i] << ")" << std::endl;
		}
	}
	else
	{
		std::cout << "No enough points detected in Radius Search!" << std::endl;
	}
	return(0);

}

三、编译结果