《PCL》kdtree

若searchPoint为待搜索点云之外，则…
若searchPoint为待搜索点云之内，则第一个点为其本身，已经验证过。

1. K搜索

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


pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
kdtree.setInputCloud(cloud);
pcl::PointXYZ searchPoint;
int K = 10;
std::vector<int>pointIdxNKNSearch(K);
std::vector<float>Distance(K);
kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, Distance) 
	
		
	

2. 半径r搜索

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

pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
kdtree.setInputCloud(cloud);
pcl::PointXYZ searchPoint;
float radius;
std::vector<int> pointIdxRadiusSearch;
std::vector<float> Distance;
kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, Distance)

3. 对点云进行索引计算
   此时，当keypoints为全部点云时，即计算索引。

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

void getIndices (pointcloud::Ptr cloudin, pointcloud keypoints, pcl::PointIndices::Ptr indices)
{
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
        kdtree.setInputCloud(cloudin);
        std::vector<float>pointNKNSquareDistance; //近邻点集的距离
        std::vector<int> pointIdxNKNSearch;

        for (size_t i =0; i < keypoints.size();i++)
        {
                kdtree.nearestKSearch(keypoints.points[i],1,pointIdxNKNSearch,pointNKNSquareDistance);
                // cout<<"the distance is:"<<pointNKNSquareDistance[0]<<endl;
                // cout<<"the indieces is:"<<pointIdxNKNSearch[0]<<endl;
                
                indices->indices.push_back(pointIdxNKNSearch[0]);     
        }
}

4.FLANN近邻查找 获取点云平均距离
只是 计算了 前一半索引号的点云距离平均值。

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
int K=2;
float allDistance=0 ,average = 0;
for(size_t i=0; i < cloud.size()/2; i++)
{
  vector<int> indicexK;
  vector<float> distanceK;
  kdtree.nearestKSearch(cloud->point[i], K, indicesK, distanceK);
  allDistance +=sqrt(distanceK[1]);
}
average = allDistance / cloud.size()/2;