原文链接：How to use a KdTree to search — Point Cloud Library 0.0 documentation
如何使用k-d树进行查找
在这篇教程中，我们将学习如何使用k-d树来搜索某个特殊点或者区域的K最近邻域。然后我们也将学习如何在用户指定的一些半径内找到所有的邻居(在本案例中是随机生成的)。
k-d树理论基础
k-d树，又称k维树，是计算机科学中用来组织表示k维空间中点集合的一种数据结构。它的本质是一棵带有其他约束的二叉查找树。k-d树对于区间搜索和最近邻搜索十分有用。由于我们处理的点云都是三维的，所以我们使用三维的k-d树。 k-d树的每个级别使用垂直于相应轴的超平面将所有子级沿特定维度拆分。 在树的根部，所有子项都将根据第一维进行拆分（即，如果第一维坐标小于根，则它将在左子树中；如果大于根，则显然会在右边的子树）。树中向下的每个级别都在下一个维度上划分，一旦所有其他级别都用尽后，将返回到第一个维度。 建造k-d树的最有效方法是使用一种分区方法，如快速排序所使用的那样，将中点放置在根上，所有具有较小一维值的事物都放置在根部，而左侧则是较大的事物。 然后，您在左右两个子树上都重复此过程，直到要分区的最后一棵树仅由一个元素组成。
测试代码:
#include<pcl/point_cloud.h>
#include<pcl/kdtree/kdtree_flann.h>
#include<iostream>
#include<vector>
#include<ctime>
using namespace std;
int
main(int argc, char** argv)
{
//使用系统时间做随机数种子
srand(time(NULL));
//创建一个PointXYZ类型点云指针
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//初始化点云数据
cloud->width = 1000;//宽为1000
cloud->height = 1;//高为1，说明为无序点云
cloud->points.resize(cloud->width * cloud->height);
//使用随机数填充数据
for (size_t i = 0; i < cloud->size(); ++i)
{
//PointCloud类中对[]操作符进行了重载，返回的是对points的引用
// (*cloud)[i].x 等同于 cloud->points[i].x
(*cloud)[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);//推进写法
}
//创建kd树对象
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
//设置点云输入,将在cloud中搜索
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最近邻搜索
int K = 10;
//使用两个vector存储搜索结果
vector<int> pointIdxNKNSearch(K);//保存下标
vector<float> pointNKNSquaredDistance(K);//保存距离的平方
cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K = " << K << endl;
/**
* 假设我们的KdTree返回超过0个最近的邻居，
* 然后它打印出所有10个离随机searchPoint最近的邻居的位置，
* 这些都存储在我们之前创建的vector中。
*/
if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
{
cout << " " << cloud->points[pointIdxNKNSearch[i]].x
<< " " << cloud->points[pointIdxNKNSearch[i]].x
<< " " << cloud->points[pointIdxNKNSearch[i]].z
<< "( squared distance: " << pointNKNSquaredDistance[i] << " )" << endl;
}
}
//基于半径的邻域搜索
//搜索结果保存在两个数组中，一个是下标，一个是距离
vector<int> pointIdxRadiusSearch;
vector<float> pointRadiusSquaredDistance;
//设置搜索半径，随机值
float radius = 256.0f* rand() / (RAND_MAX + 1.0f);
cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << endl;
/**
* 如果我们的KdTree在指定的半径内返回超过0个邻居，它将打印出这些存储在向量中的点的坐标。
*/
if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
{
cout << " " << cloud->points[pointIdxRadiusSearch[i]].x
<< " " << cloud->points[pointIdxRadiusSearch[i]].x
<< " " << cloud->points[pointIdxRadiusSearch[i]].z
<< "( squared distance: " << pointRadiusSquaredDistance[i] << " )" << endl;
}
}
return 0;
}运行结果：
K nearest neighbor search at (184.438 332.469 430.969) with K = 10
144.281 144.281 468.156( squared distance: 4229.2 )
131.875 131.875 507.156( squared distance: 9155.41 )
221.594 221.594 526.719( squared distance: 11045.1 )
280.594 280.594 462.281( squared distance: 12051.4 )
181.906 181.906 455.75( squared distance: 12089.6 )
174.063 174.063 438.438( squared distance: 12742.4 )
156.25 156.25 345.5( squared distance: 13904 )
239.75 239.75 342.5( squared distance: 15677.4 )
112.406 112.406 337.094( squared distance: 19412.5 )
251.656 251.656 334.875( squared distance: 19988.4 )
Neighbors within radius search at (184.438 332.469 430.969) with radius=225.766
144.281 144.281 468.156( squared distance: 4229.2 )
131.875 131.875 507.156( squared distance: 9155.41 )
221.594 221.594 526.719( squared distance: 11045.1 )
280.594 280.594 462.281( squared distance: 12051.4 )
181.906 181.906 455.75( squared distance: 12089.6 )
174.063 174.063 438.438( squared distance: 12742.4 )
156.25 156.25 345.5( squared distance: 13904 )
239.75 239.75 342.5( squared distance: 15677.4 )
112.406 112.406 337.094( squared distance: 19412.5 )
251.656 251.656 334.875( squared distance: 19988.4 )
286.406 286.406 368.688( squared distance: 22097.8 )
285.094 285.094 351.25( squared distance: 22762.4 )
126.563 126.563 321.594( squared distance: 22990.5 )
120.031 120.031 357.25( squared distance: 24140.6 )
30.2813 30.2813 409.563( squared distance: 25258.4 )
261.281 261.281 368.813( squared distance: 29272.2 )
262.25 262.25 366.25( squared distance: 31916.7 )
80.8438 80.8438 296.156( squared distance: 32169.3 )
133.5 133.5 576.219( squared distance: 33113.3 )
96.375 96.375 273.156( squared distance: 33118 )
369.719 369.719 458.688( squared distance: 35444.4 )
341.219 341.219 365.625( squared distance: 37148.2 )
163.844 163.844 315.5( squared distance: 37405.8 )
92.9063 92.9063 499( squared distance: 37518 )
230.844 230.844 374.563( squared distance: 38107.5 )
166.969 166.969 465.219( squared distance: 38703.1 )
244.844 244.844 413.969( squared distance: 40311.6 )
339.719 339.719 539.75( squared distance: 41017.7 )
369.656 369.656 398.75( squared distance: 41177.2 )
215.25 215.25 278.281( squared distance: 42597.7 )
41.125 41.125 293.5( squared distance: 43013.7 )
160.313 160.313 503.656( squared distance: 43635 )
358.313 358.313 317.156( squared distance: 44335.4 )
64.7813 64.7813 586.844( squared distance: 44376.4 )
323.031 323.031 283.406( squared distance: 46063.9 )
360.844 360.844 392.688( squared distance: 46767.9 )
202.5 202.5 452.5( squared distance: 49658.5 )其中主要用了两个函数最近邻搜索nearestKSearch()和基于半径的搜索radiusSearch()。
PCL中的基于k-d树搜索都是基于FLANN，来进行快速近邻搜索。
FLANN 库全称是Fast Library for Approximate Nearest Neighbors，它是目前最完整的（近似）最近邻开源库。不但实现了一系列查找算法，还包含了一种自动选取最快算法的机制。
nearestKSearch()函数
pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k,
std::vector<int> &k_indices,
std::vector<float> &k_distances) const//************************************
// Method: nearestKSearch k-近邻搜索，搜索离point最近的k个点
// 注意此方法不对输入索引进行任何检查（即index >= cloud.points.size（） || index < 0），并假定是有效（即有限）数据。
// FullName: pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch
// Access: public
// Returns: int 返回搜索到的点的个数
// Parameter: const PointT & point 搜索离point最近的k个点
// Parameter: int k 搜索离point最近的k个点
// Parameter: std::vector<int> & k_indices 搜索到的点在数据源中的下标
// Parameter: std::vector<float> & k_distances point到被搜索点的距离，与下标相对应
//************************************
源码：
//************************************
// Method: nearestKSearch k-近邻搜索，搜索离point最近的k个点
// 注意此方法不对输入索引进行任何检查（即index >= cloud.points.size（） || index < 0），并假定是有效（即有限）数据。
// FullName: pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch
// Access: public
// Returns: int 返回搜索到的点的个数
// Parameter: const PointT & point 搜索离point最近的k个点
// Parameter: int k 搜索离point最近的k个点
// Parameter: std::vector<int> & k_indices 搜索到的点在数据源中的下标
// Parameter: std::vector<float> & k_distances point到被搜索点的距离，与下标相对应
//************************************
template <typename PointT, typename Dist> int
pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k,
std::vector<int> &k_indices,
std::vector<float> &k_distances) const
{
assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
// 如果输入的k大于总点数，就让k等于总点数
if (k > total_nr_points_)
k = total_nr_points_;
//保存结果的容器总是会被resize为k
k_indices.resize (k);
k_distances.resize (k);
//使用FLANNN快速搜索
std::vector<float> query (dim_);
point_representation_->vectorize (static_cast<PointT> (point), query);
::flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k);
::flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k);
// Wrap the k_indices and k_distances vectors (no data copy)
flann_index_->knnSearch (::flann::Matrix<float> (&query[0], 1, dim_),
k_indices_mat, k_distances_mat,
k, param_k_);
// Do mapping to original point cloud ,是否映射到原始点云
if (!identity_mapping_)
{
for (size_t i = 0; i < static_cast<size_t> (k); ++i)
{
int& neighbor_index = k_indices[i];
neighbor_index = index_mapping_[neighbor_index];
}
}
return (k);
}radiusSearch()函数
//************************************
// Method: radiusSearch
// FullName: pcl::KdTreeFLANN<PointT, Dist>::radiusSearch
// Access: public
// Returns: int
// Parameter: const PointT & point
// Parameter: double radius
// Parameter: std::vector<int> & k_indices
// Parameter: std::vector<float> & k_sqr_dists
// Parameter: unsigned int max_nn
//************************************
template <typename PointT, typename Dist> int
pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_dists, unsigned int max_nn) const
{
assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
std::vector<float> query (dim_);
point_representation_->vectorize (static_cast<PointT> (point), query);
// Has max_nn been set properly?
if (max_nn == 0 || max_nn > static_cast<unsigned int> (total_nr_points_))
max_nn = total_nr_points_;
std::vector<std::vector<int> > indices(1);
std::vector<std::vector<float> > dists(1);
::flann::SearchParams params (param_radius_);
if (max_nn == static_cast<unsigned int>(total_nr_points_))
params.max_neighbors = -1; // return all neighbors in radius
else
params.max_neighbors = max_nn;
int neighbors_in_radius = flann_index_->radiusSearch (::flann::Matrix<float> (&query[0], 1, dim_),
indices,
dists,
static_cast<float> (radius * radius),
params);
k_indices = indices[0];
k_sqr_dists = dists[0];
// Do mapping to original point cloud
if (!identity_mapping_)
{
for (int i = 0; i < neighbors_in_radius; ++i)
{
int& neighbor_index = k_indices[i];
neighbor_index = index_mapping_[neighbor_index];
}
}
return (neighbors_in_radius);
}
扫一扫
878
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
