在kd树中搜索查询点半径r内所有邻居的高级伪代码是什么

部分基于评论和部分基于废弃网络寻找答案来实现它。

高层次的想法是，你用一个边界框来填充 KD 树。也就是说，树中的每个节点都存储该节点内包含的点的轴对齐边界框。

创建树时，它非常简单，像往常一样初始化树（请参阅有关该主题的 wiki 文章），但您执行以下操作：

• 在初始化节点之前，请为根节点创建一个边界框。
• 每个新节点都继承其父节点的 BB，左边的子节点的 BB 一半位于沿当前轴的中位数左侧，右子节点的 BB 位于另一半。half

查找查询点距离内的所有邻居如下：radius

• 从根节点开始，检查查询点和框之间的 BB 距离是否小于半径，如果是将子节点推送到堆栈。
• 递归直到当前节点达到最大深度，如果为最大深度且在查询点半径内，则标记该节点进行处理。

在这一点上，你知道所有有潜在候选者的叶子，所以只需遍历它们的跨度，检查各个点，所有在redius内的点都会被添加到一个列表中，然后进行宾果游戏。

下面是一个 C++ 实现：

#include <vector>
#include <algorithm>

#include "Eigen/Dense"

template <typename Vec>
struct AABB
{
    Vec min;
    Vec max;
};

template <typename Vec>
float AABBDistance(const AABB<Vec>& bb, const Vec& point)
{
    const float width = bb.max[0] - bb.min[0];
    const float height = bb.max[1] - bb.min[1];
    const float depth = bb.max[2] - bb.min[2];

    Vec center = (bb.max + bb.min) / 2;

    const float dx = std::max(abs(point[0] - center[0]) - width / 2.f, 0.f);
    const float dy = std::max(abs(point[1] - center[1]) - height / 2.f, 0.f);
    const float dz = std::max(abs(point[2] - center[2]) - depth / 2.f, 0.f);

    return sqrt(dx * dx + dy * dy + dz * dz);
}

template <typename Vec>
AABB<Vec> ComputeBoundingBox(const std::vector<Vec>& points, uint start, uint end)
{
    float min_x = points[0].x();
    float max_x = points[0].x();
    float min_y = points[0].y();
    float max_y = points[0].y();
    float min_z = points[0].z();
    float max_z = points[0].z();

    for(uint i=start; i<end; i++)
    {
        auto& point = points[i];

        min_x = std::min(point.x(), min_x);
        max_x = std::max(point.x(), max_x);

        min_y = std::min(point.y(), min_y);
        max_y = std::max(point.y(), max_y);

        min_z = std::min(point.z(), min_z);
        max_z = std::max(point.z(), max_z);
    }

    return {{min_x, min_y, min_z}, {max_x, max_y, max_z}};
}

struct KDNode
{
    uint depth;
    uint left_child;
    uint right_child;
    std::pair<uint, uint> span;
    float median_value;
};

template <typename Vec>
class KDTree
{
public:
    uint max_depth;
    std::vector<Vec> points;
    std::vector<KDNode> nodes;
    std::vector<AABB<Vec>> bounding_boxes;

public:
    KDTree(const std::vector<Vec> &points, uint max_depth);

    std::vector<Vec> FindNeighbours(const Vec& point, float radius) const;
};

template <typename Vec>
KDTree<Vec>::KDTree(const std::vector<Vec> &point_list, uint m_depth)
    : max_depth(m_depth - 1), points(point_list)
{
    std::vector<uint> stack = {0};

    const auto node = KDNode(0,
                             0,
                             0,
                             {0, points.size()},
                             0);
    nodes = {node};
    bounding_boxes = {ComputeBoundingBox(points, 0, points.size())};

    while (!stack.empty())
    {
        uint node_id = stack.back();
        auto &node = nodes[node_id];
        auto& bounding_box = bounding_boxes[node_id];
        stack.pop_back();

        auto [start, end] = node.span;
        uint axis = node.depth % 3;

        auto order_by_axis = [&axis](Vec v1, Vec v2)
        { return v1[axis] < v2[axis]; };

        std::sort(points.begin() + start, points.begin() + end, order_by_axis);

        Assert(end >= start, "");
        uint median = start + (end - start) / 2;
        node.median_value = points[median][axis];

        if (node.depth < max_depth)
        {
            AABB<Vec> bb_left = bounding_box;
            AABB<Vec> bb_right = bounding_box;
            bb_left.max[axis] = node.median_value;
            bb_right.min[axis] = node.median_value;

            node.left_child = nodes.size();
            node.right_child = nodes.size() + 1;
            const uint depth = node.depth;
            stack.push_back(nodes.size());
            nodes.push_back({depth + 1, 0, 0, {start, median}, 0});
            bounding_boxes.push_back(bb_left);
            stack.push_back(nodes.size());
            nodes.push_back({depth + 1, 0, 0, {median, end}, 0});
            bounding_boxes.push_back(bb_right);
        }
    }
}

template <typename Vec>
std::vector<Vec> KDTree<Vec>::FindNeighbours(const Vec& point, float radius) const
{
    std::vector<uint> stack = {0};
    std::vector<Vec> neighbours;

    std::vector<uint> nodes_to_check;
    while (!stack.empty())
    {
        uint node_id = stack.back();
        stack.pop_back();

        const auto& node = nodes[node_id];
        const auto& bb = bounding_boxes[node_id];

        float distance = AABBDistance(bb, point);
        if(distance > radius) continue;

        if(node.depth == max_depth)
        {
            nodes_to_check.push_back(node_id);
        }
        else
        {
            stack.push_back(node.left_child);
            stack.push_back(node.right_child);
        }
    }

    for(uint node_id : nodes_to_check)
    {
        auto& node = nodes[node_id];
        for(uint i=node.span.first; i < node.span.second; i++)
            if((point - points[i]).norm() < radius)
                neighbours.push_back(points[i]);
    }

    return neighbours;
}