pclpy OC-Tree “体素内的邻居搜索”、“K 最近邻搜索”和“半径内的邻居搜索”

一、算法原理

1.介绍

八叉树是一种用于组织稀疏 3-D 数据的基于树的数据结构。

八叉树是一种基于树的数据结构，用于管理稀疏 3-D 数据。每个内部节点正好有八个子节点。在本教程中，我们将学习如何使用八叉树在点云数据中进行空间分区和邻居搜索。

2.“体素内的邻居搜索”、“K 最近邻搜索”和“半径内的邻居搜索”

体素内的邻居搜索

这里使用的第一种搜索方法是“体素内的邻居搜索”。它将搜索点分配给相应的叶节点体素并返回点索引向量。这些指数与落在同一体素内的点有关。因此，搜索点和搜索结果之间的距离取决于八叉树的分辨率参数。

# 体素最近邻搜索
pointIdxVec = pclpy.pcl.vectors.Int()

if octree.voxelSearch(searchPoint, pointIdxVec) > 0:
    print('Neighbors within voxel search at (', searchPoint.x,
          '', searchPoint.y,
          '', searchPoint.z, ')\n')
    for i in range(len(pointIdxVec)):
        print("  ", cloud.x[pointIdxVec[i]],
              " ", cloud.y[pointIdxVec[i]],
              " ", cloud.z[pointIdxVec[i]], "\n")

K 最近邻搜索

在此示例中，K 设置为 10。“K 最近邻搜索”方法将搜索结果写入两个单独的向量。第一个 pointIdxNKNSearch 将包含搜索结果（索引引用关联的 PointCloud 数据集）。第二个向量保存搜索点和最近邻居之间的相应平方距离。

# # k最近邻搜索
k = 10
pointIdxNKNSearch = pcl.vectors.Int()
pointNKNSquaredDistance = pcl.vectors.Float()

print('K nearest neighbor search at (', searchPoint.x,
      '', searchPoint.y,
      '', searchPoint.z,
      ') with k =', k)
if octree.nearestKSearch(searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0:
    for i in range(len(pointIdxNKNSearch)):
        print("  ", cloud.x[pointIdxNKNSearch[i]],
              " ", cloud.y[pointIdxNKNSearch[i]],
              " ", cloud.z[pointIdxNKNSearch[i]],
              " (squared distance: ", pointNKNSquaredDistance[i], ")")

半径内的邻居搜索

“半径搜索中的邻居”的工作方式与“K 最近邻搜索”非常相似。它的搜索结果被写入两个单独的向量，描述点索引和平方搜索点距离。

# 半径最近邻搜索
pointIdxRadiusSearch = pcl.vectors.Int()   # 生成向量
pointRadiusSquaredDistance = pcl.vectors.Float()  # 生成向量

radius = np.random.ranf(1) * 256.0
print("Neighbors within radius search at (", searchPoint.x,
      " ", searchPoint.y, " ", searchPoint.z, ") with radius=",
      radius, '\n')
if octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0:
    for i in range(len(pointIdxRadiusSearch)):
        print("  ", cloud.x[pointIdxRadiusSearch[i]],
              " ", cloud.y[pointIdxRadiusSearch[i]],
              " ", cloud.z[pointIdxRadiusSearch[i]],
              " (squared distance: ", pointRadiusSquaredDistance[i], ")")

可视化代码

searchPointArray = cloud.xyz[pointIdxVec]  # pointIdxVec 筛选的索引
searchCloud = pcl.PointCloud.PointXYZ.from_array(searchPointArray)
viewer = pcl.visualization.PCLVisualizer("3D viewer")  # 建立一个可视化对象，窗口名 3D viewer
viewer.addPointCloud(searchCloud)  # 点云数据添加到可刷对象中
while not viewer.wasStopped():  # 展示可视化对象
    viewer.spinOnce(10)

二、代码

from pclpy import pcl
import numpy as np

if __name__ == '__main__':
    # 生成点云数据
    cloud = pcl.PointCloud.PointXYZ()
    pcl.io.loadPCDFile('res/bunny.pcd', cloud)

    # 构建OC-Tree
    resolution = 128.0
    octree = pcl.octree.OctreePointCloudSearch.PointXYZ(resolution)
    octree.setInputCloud(cloud)
    octree.addPointsFromInputCloud()
    # 生成一个索引点
    searchPoint = pcl.point_types.PointXYZ()
    searchPoint.x = cloud.xyz[0][0]  # x
    searchPoint.y = cloud.xyz[0][1]  # y
    searchPoint.z = cloud.xyz[0][2]  # z
    print(searchPoint)
    # 体素最近邻搜索
    pointIdxVec = pcl.vectors.Int()
    if octree.voxelSearch(searchPoint, pointIdxVec) > 0:
        print('体素最近邻搜索 (', searchPoint.x,
              '', searchPoint.y,
              '', searchPoint.z, ')')
        for i in range(len(pointIdxVec)):
            print("  ", cloud.x[pointIdxVec[i]],
                  " ", cloud.y[pointIdxVec[i]],
                  " ", cloud.z[pointIdxVec[i]])

    # # k最近邻搜索
    k = 10
    pointIdxNKNSearch = pcl.vectors.Int()
    pointNKNSquaredDistance = pcl.vectors.Float()

    print('K nearest neighbor search at (', searchPoint.x,
          '', searchPoint.y,
          '', searchPoint.z,
          ') with k =', k)
    if octree.nearestKSearch(searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0:
        for i in range(len(pointIdxNKNSearch)):
            print("  ", cloud.x[pointIdxNKNSearch[i]],
                  " ", cloud.y[pointIdxNKNSearch[i]],
                  " ", cloud.z[pointIdxNKNSearch[i]],
                  " (squared distance: ", pointNKNSquaredDistance[i], ")")

    # 半径最近邻搜索
    pointIdxRadiusSearch = pcl.vectors.Int()
    pointRadiusSquaredDistance = pcl.vectors.Float()

    radius = 0.01  # 搜索半径
    print("Neighbors within radius search at (", searchPoint.x,
          " ", searchPoint.y, " ", searchPoint.z, ") with radius=",
          radius, '\n')
    if octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0:
        for i in range(len(pointIdxRadiusSearch)):
            print("  ", cloud.x[pointIdxRadiusSearch[i]],
                  " ", cloud.y[pointIdxRadiusSearch[i]],
                  " ", cloud.z[pointIdxRadiusSearch[i]],
                  " (squared distance: ", pointRadiusSquaredDistance[i], ")")

三、结果

1.原点云

2.体素内的邻居搜索

3.K 最近邻搜索

4.半径内的邻居搜索

四、相关数据

更多知识看专栏。。。