KD-Tree实现算法

原理三维点云中KD-Tree的实现流程为对于当前所有输入点云点子集分别计算这些点在XYZ三个方向上的方差找到方差最大的方向将该方向作为分割轴找到所有点在该维度上的中位数根据中位数对当前点集进行分割对分割得到的左右两个子集重复上述步骤通过递归完成分割直到子集内点数达到设定阈值后停止分裂最终得到的子集就是树的叶子节点。在PCL库的KD树实现中叶子节点阈值的默认值通常为12原因是如果强制将节点拆分到每个节点仅包含一个点整棵树会非常深对CPU的开销损耗很大。实际使用时可以根据点云稠密情况调整阈值如果处理的点云比较稠密可能达到百万级可以把叶子节点阈值调大一些这样可以减少树的层数降低搜索开销如果点云非常稀疏可以把阈值稍微调小一点对空间做更精细的划分实现算法PCL 中最常用的 KD-Tree 实现是基于 FLANN 库的类名:pcl::KdTreeFLANNPointT头文件:#include pcl/kdtree/kdtree_flann.h功能: 将点云数据组织成二叉树结构将 O(N)O(N) 的线性搜索复杂度降低到 O(log⁡N)O(logN) 极大加速邻近点查找。初始化KD-Tree模板参数 pcl::PointXYZ 必须与输入点云的类型保持一致准备存储搜索结果的容器一个存储搜索到的距离一个存储每个点的索引pcl::KdTreeFLANNpcl::PointXYZkdtree;//初始化kdtreekdtree.setInputCloud(cloud);// 传入点云指针std::vectorintpointIdx;// 存储找到点的索引std::vectorfloatpointDist;// 存储找到点到查询点的距离的平方K 近邻搜索(查找最近的 K 个点)intK5;//最近邻点数量intfoundCountkdtree.nearestKSearch(searchPoint,K,pointIdx,pointDist);//返回搜索到点的个数半径搜索查半径范围内的点的个数floatradius0.5f;//搜索球半径intfoundCountRadiuskdtree.radiusSearch(searchPoint,radius,pointIdx,pointDist);//返回半径内的点数遍历找到的结果for(inti0;ifoundCount;i){intindexpointIdx[i];//获取在原点云中的序号// 获取对应点坐标floatxcloud-points[index].x;floatycloud-points[index].y;floatzcloud-points[index].z;// 获取点之间的距离floatsquared_distpointDist[i];floatreal_diststd::sqrt(squared_dist);}完整代码#includeiostream#includepcl/point_cloud.h#includepcl/point_types.h#includepcl/kdtree/kdtree_flann.hintmain(){// 创建点云pcl::PointCloudpcl::PointXYZ::Ptrcloud(newpcl::PointCloudpcl::PointXYZ);// 生成随机点云数据cloud-width1000;cloud-height1;cloud-points.resize(cloud-width*cloud-height);for(size_t i0;icloud-points.size();i){cloud-points[i].x1024.0f*rand()/(RAND_MAX1.0f);cloud-points[i].y1024.0f*rand()/(RAND_MAX1.0f);cloud-points[i].z1024.0f*rand()/(RAND_MAX1.0f);}// 创建KdTreepcl::KdTreeFLANNpcl::PointXYZkdtree;kdtree.setInputCloud(cloud);// 设置查询点pcl::PointXYZ searchPoint;searchPoint.x1024.0f*rand()/(RAND_MAX1.0f);searchPoint.y1024.0f*rand()/(RAND_MAX1.0f);searchPoint.z1024.0f*rand()/(RAND_MAX1.0f);// K近邻搜索intK10;std::vectorintpointIdxNKNSearch(K);std::vectorfloatpointNKNSquaredDistance(K);std::coutK近邻搜索 (KK)std::endl;if(kdtree.nearestKSearch(searchPoint,K,pointIdxNKNSearch,pointNKNSquaredDistance)0){for(size_t i0;ipointIdxNKNSearch.size();i){std::cout cloud-points[pointIdxNKNSearch[i]].x cloud-points[pointIdxNKNSearch[i]].y cloud-points[pointIdxNKNSearch[i]].z (距离平方: pointNKNSquaredDistance[i])std::endl;}}// 半径搜索floatradius256.0f*rand()/(RAND_MAX1.0f);std::vectorintpointIdxRadiusSearch;std::vectorfloatpointRadiusSquaredDistance;std::cout半径搜索 (半径radius)std::endl;if(kdtree.radiusSearch(searchPoint,radius,pointIdxRadiusSearch,pointRadiusSquaredDistance)0){for(size_t i0;ipointIdxRadiusSearch.size();i){std::cout cloud-points[pointIdxRadiusSearch[i]].x cloud-points[pointIdxRadiusSearch[i]].y cloud-points[pointIdxRadiusSearch[i]].z (距离平方: pointRadiusSquaredDistance[i])std::endl;}}return0;}运行结果