PCL::KdTree
对于KdTree，其内部主要有以下成员：
- input_ 指向目标点云的指针
- indices_ 指向indices的指针
- epsilon_ 误差精度
- min_pts_ 一次可行的结果要返回的最小点的数目
- sorted_ 半径查询结果是否有序
- point_representation_ 点的表示方法
PCL::KdTreeFLANN<PointT, Dist>
是KdTree的派生类。
nearestKSearch()
实际上是层层包装，调用了flann_index_内部的knnSearch函数。flann_index_是一个FLANNIndex，是::flann::Index<Dist>的别名。pcl的KdTree搜索只是对FLANN的一层包装。
radiusSearch()同理，是对flann::Index<Dist>->radiusSearch()的封装。
换而言之，整个pcl库中的kd-tree只是对FLANN的封装。
流程:
1. 创建KdTreeFLANN对象，并把创建的点云作为输入，创建一个searchPoint作为查询点
kdtree.setInputCloud (cloud);
pcl::PointXYZ searchPoint;2. 执行搜索nearestKSearch或者radiusSearch
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 ) //执行K近邻搜索
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) //执行半径R内近邻搜索方法
注：FLANN是一个快速最近邻搜索的库
#include <pcl/point_cloud.h> //点类型定义头文件
#include <pcl/kdtree/kdtree_flann.h> //kdtree类定义头文件
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char** argv)
{
srand (time (NULL)); //用系统时间初始化随机种子
//创建一个PointCloud<pcl::PointXYZ>
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); // // 产生数值为0-1024的浮点数
cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
}
//创建KdTreeFLANN对象，并把创建的点云设置为输入,创建一个searchPoint变量作为查询点
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices)
//设置搜索空间
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);
// K 临近搜索
//创建一个整数（设置为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;
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 ) //执行K近邻搜索
{
//打印所有近邻坐标
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;
}
/**********************************************************************************
下面的代码展示查找到给定的searchPoint的某一半径（随机产生）内所有近邻，重新定义两个向量
pointIdxRadiusSearch pointRadiusSquaredDistance来存储关于近邻的信息
********************************************************************************/
// 半径 R内近邻搜索方法
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;
// 假设我们的kdtree返回了大于0个近邻。那么它将打印出在我们"searchPoint"附近的10个最近的邻居并把它们存到先前创立的向量中。
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) //执行半径R内近邻搜索方法
{
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;
}
return 0;
}
pcl::search::KdTree < PointT >
pcl::search::KdTree<PointT>是pcl::search::Search< PointT >的子类，是pcl::KdTree<PointT>的包装类。包含(1) k 近邻搜索；(2) 邻域半径搜索。
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h> // 包含kdtree头文件
typedef pcl::PointXYZ PointT;
int main()
{
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::io::loadPCDFile("read.pcd", *cloud);
// 定义KDTree对象
pcl::search::KdTree<PointT>::Ptr kdtree(new pcl::search::KdTree<PointT>);
kdtree->setInputCloud(cloud); // 设置要搜索的点云，建立KDTree
std::vector<int> indices; // 存储查询近邻点索引
std::vector<float> distances; // 存储近邻点对应距离的平方
PointT point = cloud->points[0]; // 初始化一个查询点
// 查询距point最近的k个点
int k = 10;
int size = kdtree->nearestKSearch(point, k, indices, distances);
std::cout << "search point : " << size << std::endl;
// 查询point半径为radius邻域球内的点
double radius = 2.0;
size = kdtree->radiusSearch(point, radius, indices, distances);
std::cout << "search point : " << size << std::endl;
system("pause");
return 0;
}pcl::KdTreeFLANN < PointT >
pcl::KdTreeFLANN<PointT>是pcl::KdTree<PointT>的子类，可以实现同样的功能。
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
// 包含相关头文件
#include <pcl/kdtree/kdtree_flann.h>
typedef pcl::PointXYZ PointT;
int main()
{
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::io::loadPCDFile("read.pcd", *cloud);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; //创建KDtree
kdtree.setInputCloud(cloud); // 设置要搜索的点云，建立KDTree
std::vector<int> indices; // 存储查询近邻点索引
std::vector<float> distances; // 存储近邻点对应距离的平方
PointT point = cloud->points[0]; // 初始化一个查询点
// 查询距point最近的k个点
int k = 10;
int size = kdtree.nearestKSearch(point, k, indices, distances);
std::cout << "search point : " << size << std::endl;
// 查询point半径为radius邻域球内的点
double radius = 2.0;
size = kdtree.radiusSearch(point, radius, indices, distances);
std::cout << "search point : " << size << std::endl;
system("pause");
return 0;
}