k-d tree与八叉树
1 k-d tree与八叉树
本文并不涉及具体原理的解释，文章着重在k-d树与八叉树在近邻搜索方面的API的使用
1.1 k-d tree
k-d tree算法及原理： https://www.cnblogs.com/flyinggod/p/8727584.html
实现功能
- 使用k-d tree找到某个具体点的k近邻
- 在指定半径内搜索其近邻
关键API
//k近邻搜索
int pcl::search::KdTree< PointT, Tree >::nearestKSearch (
const PointT &point, //搜索点
int k, //搜索个数
std::vector<int> &k_indices, //保存近邻的索引值
std::vector<float> &k_distances //保存近邻的平方距离
)const
//半径radius内近邻搜索
int pcl::search::KdTree< PointT, Tree >::radiusSearch (
const PointT &point, //搜索点
double radius, //搜索半径
std::vector<int> &k_indices, //保存近邻的索引值
std::vector<float> &k_sqr_dists, //保存近邻的平方距离
unsigned int max_nn //默认值0,如果给定一个数值，则将返回近邻的最大值限制在这个数值以下(包括该数值)，如果给0或者高于输入点云中的个数，则返回半径中的所有近邻
)const
完整代码
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main(int argc, char** argv)
{
srand(time(NULL)); //初始化随机数种子
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// Generate pointcloud data
//随机点云生成
cloud->width = 1000; //点云数量
cloud->height = 1; //无序点云
cloud->points.resize(cloud->width * cloud->height);
for (std::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);
}
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; //创建KdTreeFLANN对象
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 nearest neighbor search
/*--------------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;
//核心函数nearestKSearch()，第三个参数的类型为pcl::Indices，进一步参看该类型可以发现为vector<int>，这也就是为什么前面pointIdxNKNSearch的类型声明为vector<int>
if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
for (std::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;
}
// Neighbors within radius search
/*--------------在半径范围内搜索所有近邻点----------------------*/
//这两个向量用来分别存储近邻的索引值、近邻的平方距离
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;
//radiusSearch最后一个参数，用于限制满足近邻条件后的结果的个数，默认值为0，代表全部输出
if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance, 3) > 0)
{
for (std::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;
}
运行结果
//k近邻搜索结果
K nearest neighbor search at (443.625 766.75 839.094) with K=10
450.813 818.625 869.688 (squared distance: 3678.65)
449.688 737.063 784.75 (squared distance: 3871.34)
445.344 825.344 864.063 (squared distance: 4059.62)
497.906 737.563 874.625 (squared distance: 5060.83)
392.219 728.281 870.125 (squared distance: 5085.39)
437.094 762.938 766.313 (squared distance: 5354.3)
463.188 814.344 780.063 (squared distance: 6132.54)
441.531 791.875 745.313 (squared distance: 9430.57)
465.75 811.781 754.781 (squared distance: 9625.93)
491.125 677.094 821 (squared distance: 10621.9)
//指定半径内的搜索结果
Neighbors within radius search at (443.625 766.75 839.094) with radius=130.57
450.813 818.625 869.688 (squared distance: 3678.65)
449.688 737.063 784.75 (squared distance: 3871.34)
445.344 825.344 864.063 (squared distance: 4059.62)
497.906 737.563 874.625 (squared distance: 5060.83)
392.219 728.281 870.125 (squared distance: 5085.39)
437.094 762.938 766.313 (squared distance: 5354.3)
463.188 814.344 780.063 (squared distance: 6132.54)
441.531 791.875 745.313 (squared distance: 9430.57)
465.75 811.781 754.781 (squared distance: 9625.93)
491.125 677.094 821 (squared distance: 10621.9)
395.406 670.344 847.875 (squared distance: 11696.3)
513.594 749.344 930.344 (squared distance: 13525.2)
447.656 660.813 898.438 (squared distance: 14760.7)
449.969 763.281 961.281 (squared distance: 14982.1)
419.344 869.75 776.75 (squared distance: 15085.3)
380.563 866.75 886.688 (squared distance: 16242)
492.063 684.906 925.594 (squared distance: 16526.8)
//结果是由小到大依次排列
对最大的距离进行验证发现
对16526开平方==128 < 130.57 可以判断所得到的结果都是在规定的搜索半径内的
1.2 八叉树
八叉树相关解释见：https://blog.csdn.net/qq_37855507/article/details/90957798
实现功能
学习使用八叉树在点云数据中进行近邻搜索
- 体素内近邻搜索
- K近邻搜索
- 半径内近邻搜搜
关键API
//[in]表示输入参数， [out]表示输出参数
bool pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::voxelSearch(
const PointT& point, //[in]搜索点
std::vector<int>& point_idx_data //[out]搜索到的近邻带你索引
)
//k近邻搜索
int pcl::search::Octree< PointT, LeafTWrap, BranchTWrap, OctreeT >::nearestKSearch(
const PointT& point, //[in]搜索点
int k, //[in]近邻点个数
std::vector<int>& k_indices, //[out]搜索到的近邻带你索引
std::vector<float>& k_sqr_distances //[out]搜索到的紧邻点的平方距离
)const
//半径搜搜
int pcl::search::Octree< PointT, LeafTWrap, BranchTWrap, OctreeT >::radiusSearch(
const PointT& p_q, //[in]搜索点
double radius, //[in]搜索半径
std::vector<int>& k_indices, //[out]搜索到的近邻带你索引
std::vector<float>& k_sqr_distances, //[out]搜索到的紧邻点的平方距离
unsigned int max_nn = 0 //[in]用来限制找到的结果的最大个数，默认为0，即报错所有结果
)const
完整代码
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main(int argc, char** argv)
{
srand((unsigned int)time(NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// Generate pointcloud data
cloud->width = 1000;
cloud->height = 1;
cloud->points.resize(cloud->width * cloud->height);
for (std::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);
}
float resolution = 256.0f; //八叉树的分辨率参数，用来描述最低一级八叉树的最小体素的尺寸
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); //初始化八叉树
octree.setInputCloud(cloud); //设置输入点云
octree.addPointsFromInputCloud(); //构建八叉树
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);
// Neighbors within voxel search
//体素内近邻搜索
std::vector<int> pointIdxVec; //用于保存体素近邻搜索的结果向量
if (octree.voxelSearch(searchPoint, pointIdxVec))
{
std::cout << "Neighbors within voxel search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z << ")"
<< std::endl;
for (std::size_t i = 0; i < pointIdxVec.size(); ++i)
std::cout << " " << cloud->points[pointIdxVec[i]].x
<< " " << cloud->points[pointIdxVec[i]].y
<< " " << cloud->points[pointIdxVec[i]].z << std::endl;
}
// K nearest neighbor search
//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 (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
for (std::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;
}
// Neighbors within radius search
//半径内近邻搜索
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;
if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
for (std::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;
}
}
运行结果
在指定的半径内没有搜索到满足条件的点