PCL学习笔记（八）-- PCL实现快速邻域搜索

一、简介

  学习如何用k-d tree树找到具体点或空间位置的k近邻，然后学习如何找到用户指定的（本例中是随机的）某一半径内的所有近邻。

  Vector：

  C++标准库提供了被封装的动态数组——Vector。大体上讲，就是更容易上手的动态数组。让我们看看Vector的优缺点。

        优点:
        1) Vector可以存放任意类型的数据
        2) 容量可以自动扩展
        3) 初始化形式简单，多样
        4) 可以插入或删除元素
        5) 可以获取数组的长度(是否为空)        
        6) 也是通过下标来访问的
        7) 能够一下清除所有数据
        8) 强制调整数组元素的个数以及数值
        9) 将数组的容量进行人为的扩大
        10) 将两个Vector数组的值进行整体性交换

        缺点: Vector的操作简单，容易上手，是数组的"升级版"，但是Vector的每次自动扩容都是增加当前空间的50%(第一次除外)；会消耗大量的空间与时间，所以小数据使用Vector还是很高效的。

二、代码分析

  1）代码首先使用系统时间初始化rand()函数的种子，利用时间初始化，每次运行时所产生的随机数都是不同的，或者用户可以用固定的数值不初始化随机种子，运行产生的随机数不变，然后创建点云对象，并用随机数据填充点云对象：

srand (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);
  }

  2）下面的代码块创建了KdTreeFLANN对象，并把我们创建的点云设置成输入，然后创建一个searchPoint变量作为查询点，并且为它分配随机坐标值：

pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;//创建kdtree对象
kdtree.setInputCloud (cloud);//设置搜索空间
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）现在创建一个整数（设置为10）和两个向量来存储搜索到的k近邻，两个向量中，一个存储搜索到查询点近邻的索引，另一个存储对应近邻的平方距离：

int K =10;
std::vector<int>pointIdxNKNSearch(K);//存储查询点近邻索引
std::vector<float>pointNKNSquaredDistance(K);//存储近邻点对应的平方距离
std::cout<<"K nearest neighbor search at ("<<searchPoint.x//打印相关信息
<<" "<<searchPoint.y
<<" "<<searchPoint.z
<<") with K="<< K <<std::endl;

  4）假设k-d tree对象返回了多余0个近邻，搜索结果已经存储在我们之前创建的两个向量pointIdxNKNSearch、pointNKNSquaredDistance中，并把所有10个近邻的位置打印输出：

if ( kdtree.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）下面代码展示找到了给定searchPoint的某一半径（随机产生）内的所有近邻，它重新定义两个向量pointIdxRadiusSearch、pointRadiusSquaredDistance来存储关于近邻的信息：

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;

  6）像之前一样，如果k-d tree对象在指定半径内返回多于0个近邻，它将打印输出向量中存储的点的坐标于距离：

if ( kdtree.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;
  }

  7）整体代码如下：

#include <iostream>
#include <vector>																			//动态数组vector的头文件
#include <ctime>																			//系统时间头文件
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>														//k近邻搜索头文件

int main(int argc, char** argv)
{
	srand(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);
	}

	pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;													//建立kd tree
	kdtree.setInputCloud(cloud);															//设置kd tree 的搜索范围
	pcl::PointXYZ searchpoint;																//设置搜索中心点，并为该店随机赋值
	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;																				//设置搜索的k近邻的数量为10
	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 (kdtree.nearestKSearch(searchpoint, K, pointIdxNKNSearch, pointNKNSquareDistance) > 0)//如果kd tree存在近邻则输出，不存在则不输出
	{
		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 (kdtree.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);

}

三、编译结果