Kd_tree、八叉树的构建查找及优化

Kd_tree 的构建思路：循环沿着x、y、z轴的方向进行排序、切分、递归

point_indices_left_end_idx = math.ceil(len(point_indices_sorted) / 2) - 1    
# 左半部分终止索引
point_sorted_middle_left = point_indices_sorted[point_indices_left_end_idx]
# 左半部分末端值
point_indices_left_end_value = db[point_sorted_middle_left, axis]  
# 排序后左半部分的索引值作为新的point_indices进行递归
point_indices_left = point_indices_sorted[0:point_indices_left_end_idx + 1]

point_indices_right_start_idx = point_indices_left_end_idx + 1  
# 右半部分的起始索引
point_sorted_middle_right = point_indices_sorted[point_indices_right_start_idx]
# 右半部分起始值
point_indices_right_start_value = db[point_sorted_middle_right, axis] 
# 排序后右半部分的索引值作为新的point_indices进行递归
point_indices_right = point_indices_sorted[point_indices_right_start_idx:]

# 计算均值作为root的value值
root.value = (point_indices_left_end_value + point_indices_right_start_value) * 0.5
# 切分轴转换
axis = axis_round_robin(axis, dim)

# 左半部分数据继续切分构建kd_tree
root.left = kdtree_recursive_build(root.left, db, point_indices_left, axis, leaf_size, dim)
# 右半部分数据继续切分构建kd_tree
root.right = kdtree_recursive_build(root.right, db, point_indices_right, axis, leaf_size, dim)

Kd_tree的查找思路：若此点在左半部分区域，优先在左子树进行寻找邻居，再根据最坏距离考虑是否到右子树继续查找可能的邻居

if query[root.axis] <= root.value:
    # root.value > query[root.axis] 说明此点在 axis 的左半部分区域，优先在左子树进行 寻找邻居
    kdtree_radius_search(root.left, db, result_set, query)
    # 如果此点据 root 的距离小于最坏距离，有必要在 root 的右子树中继续寻找可能的邻居
    if math.fabs(query[root.axis] - root.value) < result_set.worstDist():
        kdtree_radius_search(root.right, db, result_set, query)
else:
    # root.value < query[root.axis] 说明此点在 axis 的右半部分区域，优先在右子树进行 寻找邻居
    kdtree_radius_search(root.right, db, result_set, query)
    # 如果此点据 root 的距离在小于最坏距离，有必要在 root 的左子树中继续寻找可能的邻居
    if math.fabs(query[root.axis] - root.value) < result_set.worstDist():
        kdtree_radius_search(root.left, db, result_set, query)

八叉树的构建思路：先确定某一点在正方体中的八个分区之一，然后计算八个小正方体分区的体心，之后进行递归

root.is_leaf = False
# 构建正方体八个区域
region = [[] for i in range(8)]
# 分别将点映射到正方体八个区域之一
# 以中心点的坐标和迭代点的坐标的关系为条件通过region_index是否和001、010、100进行与运算分别找到每个点所属的区域编号
for idx in point_indices:
    db_point = db[idx]
    region_index = 0  # 区域号
    if db_point[0] > center[0]:
        region_index = region_index | 1
    if db_point[1] > center[1]:
        region_index = region_index | 2
    if db_point[2] > center[2]:
        region_index = region_index | 4
    region[region_index].append(idx)
factor = [-0.5, 0.5]
# 分别在每个子区域中进行octree的构建
for i in range(8):
    region_every_center_x = center[0] + factor[(i&1) > 0] * extent  # 子区域中心点坐标的x
    region_every_center_y = center[0] + factor[(i&2) > 0] * extent  # 子区域中心点坐标的y
    region_every_center_z = center[0] + factor[(i&4) > 0] * extent  # 子区域中心点坐标的z
    region_every_center = np.asarray([region_every_center_x, region_every_center_y, region_every_center_z])
    region_every_extent = 0.5 * extent
    # 递归法进行子树的构建
    root.children[i] = octree_recursive_build(root.children[i],
                                              db,
                                              region_every_center,
                                              region_every_extent,
                                              region[i],
                                              leaf_size,
                                              min_extent)

八叉树的查找思路：首先确定某个点所属的分区，然后优先在所属分区中查找点的邻居

# 判断此点的归属区域 
morton_code = 0
if query[0] > root.center[0]:
    morton_code = morton_code | 1
if query[1] > root.center[1]:
    morton_code = morton_code | 2
if query[2] > root.center[2]:
    morton_code = morton_code | 4

# 在计算出的点的归属区域中寻找可能存在的邻居
if octree_radius_search(root.children[morton_code], db, result_set, query):
    return True
    
# 寻找其他的可能邻居
for c, child in enumerate(root.children):
    if c == morton_code or child is None:
        continue
    if False == overlaps(query, result_set.worstDist(), child):
        continue
    if octree_radius_search(child, db, result_set, query):
        return True

主要优化措施： Kd_tree: 考虑到 kd_tree 建树时每一次递归都需要排序，鉴于每一次排序后数据延着每一个轴向的有序性，拟采用每一次排序后切分两次的办法保证建树质量的同时试着提高建树的速度 两次换一次切分轴，主要重写函数 axis_round_robin

def axis_round_robin(axis, dim):
    # 全局变量
    global axis0_use    # 轴 0 的使用次数统计
    global axis1_use    # 轴 1 的使用次数统计
    global axis2_use    # 轴 2 的使用次数统计
    # 全局变量
    
    if axis == 0:
        axis0_use += 1
        if axis0_use == 3:
            axis0_use = 0
            axis += 1
    if axis == 0 and (axis0_use == 1 or axis0_use == 2):
        return axis

    if axis == 1:
        axis1_use += 1
        if axis1_use == 3:
            axis1_use = 0
            axis += 1
    if axis == 1 and (axis1_use == 1 or axis1_use == 2):
        return axis

    if axis == 2:
        axis2_use += 1
        if axis2_use == 3:
            axis2_use = 0
            axis = 0
    if axis == 2 and (axis2_use == 1 or axis2_use == 2):
        return axis

然而改写axis_round_robin函数后程序运行时，当数据量比较少的时候切分效果及速度比较好，一旦数据量变大时，总会出现隐藏的错误维度出现，因此程序含有一些未知的错误，之后我将继续延着此思路调试程序，继续学习！！！
希望对无人驾驶感兴趣的同学一起交流、共同进步啊！！！让我们一起越走越远……