PCL学习2：使用KDtree进行搜索

KDtree是点云匹配的常用方法，原理：
KDTree 解释与实现
KDtree简介

PCL已经实现了该功能，官方文档：PCL KDtree

类KdTreeFLANN

class pcl::KdTreeFLANN< PointT, Dist >
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
The class is making use of the FLANN (Fast Library for Approximate Nearest Neighbor) project by Marius Muja and David Lowe.

常用的成员函数
1、setInputCloud

void pcl::KdTreeFLANN< PointT, Dist >::setInputCloud (const PointCloudConstPtr & 	cloud,
const IndicesConstPtr &indices = IndicesConstPtr () 
)	

Provide a pointer to the input dataset.//为kdtree设置待搜索点云
Parameters
[in] cloud the const boost shared pointer to a PointCloud message//点云指针
[in] indices the point indices subset that is to be used from cloud - if NULL the whole cloud is used

2、nearestKSearch（有多个重载版本，这里只给出一个）

int pcl::KdTreeFLANN< PointT, Dist >::nearestKSearch (const PointT &point, 
int k,
std::vector< int > & 	k_indices,
std::vector< float > & 	k_sqr_distances 
)	

Search for k-nearest neighbors for the given query point.
Attention
This method does not do any bounds checking for the input index (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
Parameters
[in] point a given valid (i.e., finite) query point//待搜索的点
[in] k the number of neighbors to search for //设定k值，以寻找待搜索点的k个最近临点
[out] k_indices the resultant indices of the neighboring points (must be resized to k a priori!)//k个最近临点的索引，需要提前对vector进行resize至指定大小（即k）
[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to k a priori!)//k个最近临点到搜索点距离的平方，同样需要提前对vector进行resize至指定大小
Returns
number of neighbors found//返回值：找到的最近临点个数

3、radiusSearch（有多个重载版本，这里只给出一个）

int pcl::KdTreeFLANN< PointT, Dist >::radiusSearch (const PointT & 	point,
double 	radius,
std::vector< int > & 	k_indices,
std::vector< float > & 	k_sqr_distances,
unsigned int 	max_nn = 0 
)	

Search for all the nearest neighbors of the query point in a given radius.
Attention
This method does not do any bounds checking for the input index (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
Parameters
[in] point a given valid (i.e., finite) query point//待搜索点
[in] radius the radius of the sphere bounding all of p_q’s neighbors//搜索半径
[out] k_indices the resultant indices of the neighboring points//vector，用于存放搜索到的点的索引
[out] k_sqr_distances the resultant squared distances to the neighboring points
[in] max_nn if given, bounds the maximum returned neighbors to this value. If max_nn is set to 0 or to a number higher than the number of points in the input cloud, all neighbors in radius will be returned.
Returns
number of neighbors found in radius

用法示例

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

//省略点云io及预处理操作
int k = 10；
double radius = 5;
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;//定义kdtree结构
kdtree.setInputCloud(cloud_to_search);//设置待搜索点云
pcl::PointXYZ search_point = cloud->points[i];//设置搜索点
std::vector<int> index(k);//提前给vector申请空间
std::vector<float> dist(k);
kdtree.nearestKSearch(search_point, k, index, dist);
//也可以按照搜索半径进行搜索，此时vector不需要提前申请空间
//kdtree.radiusSearch(search_point, radius, index, dist);