Open3D KDtree的建立与使用

一、概述

1.1kd树原理

1.2kd树搜索原理

1.3kd树构建示例

二、常见的领域搜索方式

2.1K近邻搜索（K-Nearest Neighbors, KNN Search）

2.2半径搜索（Radius Search）

2.3混合搜索（Hybrid Search）

三、代码实现

3.1关键函数

3.1.1K近邻搜索

3.1.2半径邻域搜索

3.1.3混合搜索

3.2完整代码

四、实现效果

一、概述

1.1kd树原理

KD树（K-Dimensional Tree）是一种用于组织k维空间数据的树状数据结构，特别适用于多维空间中的最近邻搜索和范围搜索。KD树通过递归地将空间划分为较小的子空间，从而实现高效的空间查询。

KD树的构建原理：

选择分割维度从数据集中选择一个维度进行分割。通常选择当前维度上的方差最大的维度，以最大化分割的效果。这可以帮助平衡树的结构。
选择分割点：在选择的分割维度上选择中位数作为分割点。中位数确保每次分割后，两个子空间包含的点数大致相等，从而保持树的平衡。
递归构建子树：对于每个子集，递归地选择新的分割维度和分割点，直到达到某个终止条件，例如节点包含的点数小于某个阈值或树的深度达到预定值。

1.2kd树搜索原理

1.最近邻搜索：

从根节点开始，根据查询点在当前分割维度上的值，递归地搜索子树，直到到达叶节点。
在回溯过程中，检查当前节点是否比已知的最近邻更近，如果是，则更新最近邻。
还需检查当前节点的另一子树是否可能包含更近的点，如果可能，则进行搜索。

2.范围搜索：

类似于最近邻搜索，通过比较查询点与分割点的关系，递归地搜索子树，检查节点是否在查询范围内。

1.3kd树构建示例

我们将使用以下点构建一个KD树：

A(2,3), B(5,4), C(9,6), D(4,7), E(8,1), F(7,2)

第一层：

选择 x 轴进行分割。
选择 x 轴上的中位数作为分割点，这里是点 D(4,7)。

                D(4,7)/      \

第二层：

对于左子树，选择 y 轴进行分割。
左子树的点为 A(2,3) 和 B(5,4)，选择 y 轴上的中位数点 A(2,3) 作为分割点。
对于右子树，选择 y 轴进行分割。
右子树的点为 C(9,6), E(8,1) 和 F(7,2)，选择 y 轴上的中位数点 F(7,2) 作为分割点。

                D(4,7)/      \A(2,3)     F(7,2)\       /    \B(5,4) E(8,1) C(9,6)

第三层：

对于左子树的右子树，选择 x 轴分割。
对于右子树的左右子树，选择 x 轴分割。

最终构建的KD树结构如下：

                D(4,7)/      \A(2,3)     F(7,2)\       /    \B(5,4) E(8,1) C(9,6)

二、常见的领域搜索方式

2.1K近邻搜索（K-Nearest Neighbors, KNN Search）

K近邻搜索是找到离查询点最近的K个点的一种方法。K近邻搜索基于欧几里得距离度量，通过KD树可以高效地实现。

过程：

从根节点开始，根据查询点在当前分割维度上的值，递归地搜索子树，直到到达叶节点。
在回溯过程中，检查当前节点是否比已知的K个最近邻点更近，如果是，则更新最近邻集合。
还需检查当前节点的另一子树是否可能包含更近的点，如果可能，则进行搜索。

应用：

数据分类：KNN算法在分类问题中广泛应用，通过查找最近的K个邻居进行多数投票决定分类结果。
数据降噪：可以通过找到每个点的K个最近邻来平滑数据。

2.2半径搜索（Radius Search）

半径搜索是找到所有在查询点某个给定半径范围内的点的一种方法。与K近邻搜索不同，半径搜索返回的是所有在指定半径范围内的点。

过程：

从根节点开始，根据查询点和分割点之间的距离，递归地搜索子树。
检查当前节点是否在查询点的半径范围内，如果是，则将其加入结果集合。
检查当前节点的另一子树是否可能包含在半径范围内的点，如果可能，则进行搜索。

应用：

密度估计：通过找到某个区域内的所有点，可以估计该区域的点云密度。
空间聚类：在聚类算法中，半径搜索用于找到每个点的邻域，从而进行聚类。

2.3混合搜索（Hybrid Search）

混合搜索结合了K近邻搜索和半径搜索的特点，在进行K近邻搜索的同时，还限制了搜索范围在一个给定的半径内。也就是说，它在指定半径范围内找到最多K个最近的点。

过程：

从根节点开始，根据查询点在当前分割维度上的值和半径约束，递归地搜索子树，直到到达叶节点。
检查当前节点是否在查询点的半径范围内，并且是否属于最近的K个点，如果是，则将其加入结果集合。
检查当前节点的另一子树是否可能包含在半径范围内并且属于最近的K个点，如果可能，则进行搜索。

应用：

提高搜索效率：在处理大规模点云数据时，混合搜索可以限制搜索范围，从而提高搜索效率。
平衡搜索结果：混合搜索可以在保证结果精确度的同时，限制搜索范围，避免返回过多不相关的点。

三、代码实现

3.1关键函数

3.1.1K近邻搜索

search_knn_vector_3d返回查询点的k个最近邻的索引列表。这些相邻的点存储在数组numpy中，使用pcd.colors对numpy数组内所有的点进行颜色渲染（渲染为绿色[0，1，0]）。这里跳过了第一个索引点，因为它是查询点本身

#K近邻搜索
pcd.colors[10000] = [1, 0, 0]#给定查询点并渲染为红色
[k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.points[10000], 200)#K近邻搜索
np.asarray(pcd.colors)[idx[1:], :] = [0, 1, 0]#K邻域的点，渲染为绿色

3.1.2半径邻域搜索

使用 search_radius_vector_3d查询所有的和查询点点距离小于给定半径的点

#半径搜索
pcd.colors[5000] = [1, 0, 0]#给定查询点并渲染为红色
[k1, idx1, _] = pcd_tree.search_radius_vector_3d(pcd.points[5000], 0.02)#半径搜索
np.asarray(pcd.colors)[idx1[1:], :] = [0, 0, 1]#半径搜索结果并渲染为蓝色

3.1.3混合搜索

除了KNN搜索(search_knn_vector_3d)和RNN搜索(search_radius_vector_3d)以外,Open3d还提供了混合搜索函数(search_hybrid_vector_3d)。它最多返回K个和查询点距离小于给定半径的最邻近点。这个函数结合了KNN和RNN的搜索条件，在某些文献中也被称作RKNN搜索。在许多情况下它有着性能优势,并且在Open3d的函数中大量的使用.

#混合搜索
pcd.colors[30000] = [1, 1, 0]#给定查询点并渲染为黄色
[k2, idx2, _] = pcd_tree.search_hybrid_vector_3d(pcd.points[30000], 0.05,200)#K近邻搜索
np.asarray(pcd.colors)[idx2[1:], :] = [0, 1, 0.8]#半径搜索结果并渲染为青色

3.2完整代码

import open3d as o3d
import numpy as np
pcd = o3d.io.read_point_cloud("Horse.pcd")
pcd.paint_uniform_color([0.5, 0.5, 0.5])#把所有点渲染为灰色
pcd_tree = o3d.geometry.KDTreeFlann(pcd)#建立KD树索引#K近邻搜索
pcd.colors[10000] = [1, 0, 0]#给定查询点并渲染为红色[k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.points[10000], 200)#K近邻搜索
np.asarray(pcd.colors)[idx[1:], :] = [0, 1, 0]#K邻域的点，渲染为绿色#半径搜索
pcd.colors[5000] = [1, 0, 0]#给定查询点并渲染为红色
[k1, idx1, _] = pcd_tree.search_radius_vector_3d(pcd.points[5000], 0.02)#半径搜索
np.asarray(pcd.colors)[idx1[1:], :] = [0, 0, 1]#半径搜索结果并渲染为蓝色#混合搜索
pcd.colors[30000] = [1, 1, 0]#给定查询点并渲染为黄色
[k2, idx2, _] = pcd_tree.search_hybrid_vector_3d(pcd.points[30000], 0.05,200)#K近邻搜索
np.asarray(pcd.colors)[idx2[1:], :] = [0, 1, 0.8]#半径搜索结果并渲染为青色
o3d.visualization.draw_geometries([pcd])