激光雷达（LiDAR）| Open3D：第二节 邻近搜索之构建KDTree

本节将介绍Open3D开源库：KDTree的原理及构建，将无序点云变为有序点云，实现点云的快速邻近搜索。

---

1.KDTree 概述

Open3D使用FLANN构建KDTrees，将无序点云变为有序点云，以便快速检索最近邻。
在激光雷达中，一般使用的是三维点云。所以，kd-tree的维度是3。
KDTree（k 维树）是一种空间分区数据结构，它将一组 k 维点存储在树结构中，从而实现高效的范围搜索和最近邻搜索。最近邻搜索是处理点云数据时的核心操作，可用于查找点组或要素描述符之间的对应关系，或定义一个或多个点周围的局部邻域。

具体KD-Tree原理详解参考以下文章：

• KD-Tree原理详解：https://zhuanlan.zhihu.com/p/112246942
• 三维点云学习（2）中-Kd-tree (k-dimensional tree)：https://blog.csdn.net/weixin_41281151
  
  

2.KDTree 算法流程

2.1 构建KDtree

首先，读取点云并渲染为灰色便于后面显示搜索到的点云，通过o3d.geometry.KDTreeFlann函数建立树结构

pcd = o3d.io.read_point_cloud("./tree/Tree_singel.pcd")
o3d.visualization.draw_geometries([pcd],width = 700, height = 900)
pcd.paint_uniform_color([0.5, 0.5, 0.5])  # 渲染为灰色
o3d.visualization.draw_geometries([pcd],width = 700, height = 900)
pcd_tree = o3d.geometry.KDTreeFlann(pcd)  # 建立树结构

2.2 搜索相邻点

将第480000个点作为锚点并渲染成红色

pcd.colors[48000] = [1, 0, 0]  # 将该点作为锚点并渲染为红色

2.3 邻近搜索

法1：KNN最近邻近搜索

采用search_knn_vector_3d函数实现，返回锚点的 k 个最近邻的索引列表。使用np.asarray转换为 numpy 数组以批量访问点颜色，将相邻点渲染成蓝色。搜索时会跳过第一个索引，因为它是锚点本身。

# 方法1：KNN领域搜索
pcd.colors[48000] = [1, 0, 0]  # 将该点作为锚点并渲染为红色
[k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.points[48000], 300)
np.asarray(pcd.colors)[idx[1:], :] = [0, 0, 1] # 渲染成蓝色

法2：RNN半径邻近搜索

采用search_radius_vector_3d函数查询到锚点的距离小于给定半径的所有点。

# 方法2：RNN半径领域搜索
pcd.colors[2500] = [1, 0, 0]  # 将该点作为锚点并渲染为红色
[k1, idx1, _] = pcd_tree.search_radius_vector_3d(pcd.points[2500], 0.2) # 半径搜索
np.asarray(pcd.colors)[idx1[1:], :] = [0, 1, 0] # 半径搜索结果并渲染为绿色
o3d.visualization.draw_geometries([pcd], width = 700, height = 900)

法3：RKNN混合搜索

采用search_hybrid_vector_3d函数实现，结合 KNN 搜索和 RNN 搜索的条件最多返回K个和锚点距离小于给定半径的最邻近点。在许多实际情况下具有性能优势，并在Open3D的许多函数中大量使用。

# 法3：混合搜索
pcd.colors[222] = [1, 0, 0]
[k2, idx2, _] = pcd_tree.search_hybrid_vector_3d(pcd.points[222], 0.5,200) # RKNN混合搜索
np.asarray(pcd.colors)[idx2[1:], :] = [0, 1, 0.8]#半径搜索结果并渲染为青色
o3d.visualization.draw_geometries([pcd], width = 700, height = 900)

总结

本节介绍采用开源点云处理库Open3D之构建KDtree树实现点云的快速邻近搜索。

---