关于kd-tree 学习的链接:
https://en.wikipedia.org/wiki/K-d_tree
最近邻搜索
PCL学习路线
https://www.yuque.com/huangzhongqing/pcl/uffamg
1介绍
在计算机科学中,KD树(简称k维树)是空间划分的数据结构来组织点在k维空间。k-d树是多种应用的有用数据结构,例如涉及多维搜索关键字的搜索(例如范围搜索和最近邻搜索)和创建点云。k-d 树是二元空间分区树的特例。
KD树是一个二叉树,其中每个节点是一个k维点。每个非叶节点都可以被认为是隐式生成一个分裂超平面,将空间分成两部分,称为半空间。该超平面左侧的点由该节点的左子树表示,超平面右侧的点由右子树表示。超平面方向的选择方式如下:树中的每个节点都与k 个节点之一相关联维度,超平面垂直于该维度的轴。因此,例如,如果对于特定的拆分选择了“x”轴,则子树中“x”值小于节点的所有点将出现在左子树中,而“x”值较大的所有点将是在右子树中。在这种情况下,超平面将由点的 x 值设置,其法线将是单位 x 轴。
python实现:
from collections import namedtuple
from operator import itemgetter
from pprint import pformat
class Node(namedtuple("Node", "location left_child right_child")):
def __repr__(self):
return pformat(tuple(self))
def kdtree(point_list, depth: int = 0):
if not point_list:
return None
k = len(point_list[0]) # assumes all points have the same dimension
# Select axis based on depth so that axis cycles through all valid values
axis = depth % k
# Sort point list by axis and choose median as pivot element
point_list.sort(key=itemgetter(axis))
median = len(point_list) // 2
# Create node and construct subtrees
return Node(
location=point_list[median],
left_child=kdtree(point_list[:median], depth + 1),
right_child=kdtree(point_list[median + 1 :], depth + 1),
)
def main():
"""Example usage"""
point_list = [(7, 2), (5, 4), (9, 6), (4, 7), (8, 1), (2, 3)]
tree = kdtree(point_list)
print(tree)
if __name__ == "__main__":
main()
上述实现的原理,在每个维度上进行中值查找,将小于中值的作为左子树,大于的作为右子树。接着在各维度上交替进行,直到拆分结束,过程如下
2实例
kdtree_search.cpp
#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>);
//点云生成
cloud->width =1000; //点云数量
cloud->height =1; //无序点云
cloud->points.resize (cloud->width * cloud->height);
for (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; //创建kd-tree对象
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;
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 ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) >0 )
//执行k近邻搜索
{
for (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;
}
// 在半径r内搜索近邻
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 ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) >0 )
{
for (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;
}
system("PAUSE");
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(kdtree_search)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(kdtree_search kdtree_search.cpp)
target_link_libraries(kdtree_search ${PCL_LIBRARIES})
建立build空文件夹,如下:
cd build
cmake ..
make
./kdtree_search
执行结果:
源码解释
代码首先用系统时间初始化rand()函数的种子,利用时间初始化,每次运行时所产生的随机数都是不同的,或者用户可以用固定的数值或不进行初始化随机种子,运行程序产生的随机数不变,然后创建点云对象,并用随机数据填充点云对象。
下面的代码块创建了KdTreeFLANN对象,并把创建的点云设置成输入,然后创建一个searchPoint变量作为查询点,并且分配随机坐标值给它。
现在创建一个整数(设置成10)和两个向量来存储搜索到的K近邻,两个向量中一个存储搜索到查询点近邻的索引,另一个存储对应近邻的距离平方。
假设kd-tree对象返回了多于0个近邻,搜索结果已经存储在之前创建的两个向量pointIdxNKNSearch , pointNKNSquaredDistance中,并把所有10个近邻的位置打印输出。
下面代码展示查找到给定searchPoint的某一半径(随机产生)内的所有近邻,重新定义两个向量pointIdxRadiusSearch, pointRadiusSquaredDistance来存储关于近邻的信息。
像之前一样,如果 kd-tree对象在指定半径内返回多于0个近邻,它将打印输出向量中存储的点的坐标与距离。