PCL中 的 kd-tree

PCL 中的 kd-tree ，使用的是 FLANN 项目中的数据结构，支持《快速最近邻搜索》。

 

kd-tree 简称k维树，它是一种空间划分数据结构，它将一组 k 维点存储在一个树结构中，可以理解是 k 维的二叉树。

kd-tree 可以实现有效的《范围搜索》和《最近邻搜索》。

《最近邻搜索》是处理点云数据的核心操作，可用于查找点组或特征描述符之间的对应关系，或者，定义一个或多个点周围的局部邻域。

PCL 中的 kd-tree 位于 common 模块（pcl_common.dll）。

使用时要引用以下头文件：

#include pcl/kdtree/kdtree_flann.h>

以下用一个例子练习一下如何使用 kd-tree 进行点云的《最近邻搜索》。

打开 QT Creator ，然后，新建 QT Application 项目。

在项目文件 .pro 文件中添加如下包含目录和引用的 lib 库

INCLUDEPATH += C:\PCL1.12.1\include\pcl-1.12 
  C:\PCL1.12.1\3rdParty\FLANN\include \
  C:\PCL1.12.1\3rdParty\Boost\include\boost-1_78 \
  C:\PCL1.12.1\3rdParty\Eigen\eigen3 \
win32:LIBS += $$quote(C:\PCL1.12.1\lib\pcl_commond.lib)
win32:LIBS += $$quote(C:\PCL1.12.1\3rdParty\FLANN\lib\flann_s.lib)
win32:LIBS += $$quote(C:\PCL1.12.1\3rdParty\FLANN\lib\flann_cpp_s.lib)

在 main.cpp 的顶部添加头文件引用：

#include <QApplication>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

在 main.cpp 的 main 函数中输入如下代码：

//创建6个点的点云
pcl::PointCloudpcl::PointXYZ::Ptr clound(new pcl::PointCloudpcl::PointXYZ());
clound->width = 6;
clound->height = 1;
clound->resize(clound->width *clound->height);
for(int i = 0; i < clound->points.size(); ++i)
{
    clound->points[i].x = (i * 3) + 1;
    clound->points[i].y = (i * 3) + 10;
    clound->points[i].z = (i * 3) + 100;
}
//打印点云
qDebug() << "Clound : ";
for(int i = 0; i < clound->points.size(); ++i)
{
    qDebug() << clound->points[i].x << "," << clound->points[i].y << "," << clound->points[i].z;
}

//创建kd-tree对象，并将点云对象赋值给它
pcl::KdTreeFLANNpcl::PointXYZ tree;
tree.setInputCloud(clound);

//最近邻搜索

//要搜索的点
pcl::PointXYZ pt;
pt.x = 1.5;
pt.y = 10.5;
pt.z = 100.5;

//k近邻搜索
int K = 3;//最近的3个点
std::vector<int> indices;//最近的点的索引
std::vector<float> sqr_distance;//最近的点离搜索点的平方距离
//执行k近邻搜索
tree.nearestKSearch(pt, K, indices, sqr_distance);
//打印搜索结果
qDebug() << "Indices : ";
for(int i = 0; i < indices.size(); ++i)
{
    qDebug() << indices[i];
}
qDebug() << "sqr_distance : ";
for(int i = 0; i < sqr_distance.size(); ++i)
{
    qDebug() << sqr_distance[i];
}

//半径近邻搜索
double raius = 5;//距离为5
indices.clear();//最近的点的索引
sqr_distance.clear();//最近的点离搜索点的平方距离
//执行半径近邻搜索
tree.radiusSearch(pt, raius, indices, sqr_distance);
//打印搜索结果
qDebug() << "Indices : ";
for(int i = 0; i < indices.size(); ++i)
{
    qDebug() << indices[i];
}
qDebug() << "sqr_distance : ";
for(int i = 0; i < sqr_distance.size(); ++i)
{
    qDebug() << sqr_distance[i];
}

我使用的是编译器是MSC++Compiler16（VS2019）：

用该编译器时，PCL中有1个位置有一个bug，对vs的编译器漏写了一个变量，编译项目时，会提示错误，这时，需要修改后才能通过编译：

（1）修改 dist.h 文件

文件路径：C:\PCL1.12.1\3rdParty\FLANN\include\flann\algorithms\dist.h。

另外，QT6.3 创建的项目，默认使用 C++17，.pro文件中有如下行：

PCL1.12.1 使用的 FLANN 项目代码中有2处被C++17弃用的STL函数，项目编译时也会提示错误，这时，需要手动修改：

（1）std::binary_function 函数

调用该函数的文件是：

C:\PCL1.12.1\3rdParty\FLANN\include\flann\util\heap.h

直接注释掉即可：

（2）std::random_shuffle 函数

调用该函数的文件有3个：

C:\PCL1.12.1\3rdParty\FLANN\include\flann\util\random.h

C:\PCL1.12.1\3rdParty\FLANN\include\flann\algorithms\kdtree_index.h

C:\PCL1.12.1\3rdParty\FLANN\include\flann\util\lsh_table.h

三个文件中都把 std::random_shuffle 函数调用改为 std::shuffle 函数调用即可，如下：

（2.1）修改 random.h

第一步，头文件添加 #include ：

第二步，把 std::random_shuffle 函数调用改为 std::shuffle 函数调用：

（2.2）修改 kdtree_index.h

第一步，头文件添加 #include ：

第二步，把 std::random_shuffle 函数调用改为 std::shuffle 函数调用：

（2.3）修改 lsh_table.h

第一步，头文件添加 #include ：

第二步，把 std::random_shuffle 函数调用改为 std::shuffle 函数调用：

以上的例子编译成功后，运行可以得到如下输出：

Clound :
1 , 10 , 100
4 , 13 , 103
7 , 16 , 106
10 , 19 , 109
13 , 22 , 112
16 , 25 , 115
Indices :
0
1
2
sqr_distance :
0.75
18.75
90.75
Indices :
0
1
sqr_distance :
0.75
18.75

 如上所示，K近邻搜索得到，离搜索点最近的3个点的平方距离是 0.75、18.75、90.75 ，可见之后的半径为 5 的半径搜索只能搜索到最大 5*5=25 距离的点，因此，结果就是如图所示的 0.75 和 18.75 这 2 个点。