PCL——2.kd-tree

关于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

执行结果：

扫描二维码关注公众号，回复： 13192533 查看本文章

源码解释

代码首先用系统时间初始化rand()函数的种子,利用时间初始化,每次运行时所产生的随机数都是不同的,或者用户可以用固定的数值或不进行初始化随机种子,运行程序产生的随机数不变,然后创建点云对象,并用随机数据填充点云对象。

下面的代码块创建了KdTreeFLANN对象,并把创建的点云设置成输入,然后创建一个searchPoint变量作为查询点,并且分配随机坐标值给它。

现在创建一个整数(设置成10)和两个向量来存储搜索到的K近邻,两个向量中一个存储搜索到查询点近邻的索引,另一个存储对应近邻的距离平方。

假设kd-tree对象返回了多于0个近邻,搜索结果已经存储在之前创建的两个向量pointIdxNKNSearch , pointNKNSquaredDistance中,并把所有10个近邻的位置打印输出。

下面代码展示查找到给定searchPoint的某一半径(随机产生)内的所有近邻,重新定义两个向量pointIdxRadiusSearch, pointRadiusSquaredDistance来存储关于近邻的信息。

像之前一样,如果 kd-tree对象在指定半径内返回多于0个近邻,它将打印输出向量中存储的点的坐标与距离。