激光雷达定位与建图-最近邻问题2

最新推荐文章于 2025-06-02 10:31:13 发布

原创最新推荐文章于 2025-06-02 10:31:13 发布 · 1k 阅读

23 ·

CC 4.0 BY-SA版权

文章标签：

#激光雷达定位与建图 #K-D树 #k-近邻算法

激光雷达定位与建图专栏收录该内容

3 篇文章

订阅专栏

一、问题引出

最近邻问题：假设有两个点云集合， $χ1={x1,⋯xn}\chi _{1} = \left \{ x_{1},\cdots x_{n} \right \}$ ， $χ2={x1,⋯xn}\chi _{2} = \left \{ x_{1},\cdots x_{n} \right \}$ ，求点云集合 $χ2\chi _{2}$ 中某个点，在点云集合 $χ1\chi _{1}$ 中与它最近的点是？或者在点云集合中与它最近的K个点是？

常用解决方法有：暴力最近邻法、栅格与体素方法、二叉树与k-d树、四叉树与八叉树和其它树类方法。

该篇文章主要介绍：二叉树与k-d树，当然k-d树是二叉树的高维版本，而点云属于三维空间，所以本文主要介绍k-d树。

二、k-d树

k-d树是二叉树的一种，任意一个k-d树的节点由左右两侧组成。

1.k-d树创建

在构建KD树时，我们通常有两种主要的方法来选择分割轴和分割点：
1.以固定顺序交替坐标值；
2.计算当前点云在各轴上的分散程度，并取分散程度最大的轴作为分割轴。
考虑到了点云数据的实际分布情况，本文选择第二种方式构建k-d树。
要按照计算当前点云在各轴上的分散程度，并取分散程度最大的轴作为分割轴的方法来构建KD树，我们可以遵循以下步骤：

a. 准备阶段

输入数据：假设有点云数据集为 $X = \{x_1, x_2, ... , x_N\} )$ 。

b. 计算每个轴的分散程度

对于每个维度（轴），计算数据点在该维度上的方差或其他分散程度的度量。方差越大，表示数据在该维度上的分散程度越高。

c. 选择分割轴

选择维度：选择方差最大的维度作为分割轴。

d. 确定分割点

在选定的分割轴上，找到中位数作为分割点。

e. 划分数据集

根据分割点，将数据集划分为两个子集：
- 左子集：所有在分割轴上小于分割点的点。
- 右子集：所有在分割轴上大于等于分割点的点。

f. 递归构建子树

对左右子集递归执行步骤2-5，直到每个子集中只有一个数据点或达到预设的终止条件。

g. 终止条件

当某一子集为空或只包含一个数据点时，递归终止。

h. 构建完成

所有递归调用完成后，KD树构建完成。

根据kd树构建步骤，给出代码示例如下：

/// Kd树节点，二叉树结构，内部用祼指针，对外一个root的shared_ptr
struct KdTreeNode {
    int id_ = -1;
    int point_idx_ = 0;            // 点的索引
    int axis_index_ = 0;           // 分割轴
    float split_thresh_ = 0.0;     // 分割位置
    KdTreeNode* left_ = nullptr;   // 左子树
    KdTreeNode* right_ = nullptr;  // 右子树

    bool IsLeaf() const { return left_ == nullptr && right_ == nullptr; }  // 是否为叶子
};

bool KdTree::BuildTree(const CloudPtr &cloud) {
    if (cloud->empty()) {
        return false;
    }

    cloud_.clear();
    cloud_.resize(cloud->size());
    for (size_t i = 0; i < cloud->points.size(); ++i) {
        cloud_[i] = ToVec3f(cloud->points[i]);
    }

    Clear();
    Reset();

    IndexVec idx(cloud->size());
    for (int i = 0; i < cloud->points.size(); ++i) {
        idx[i] = i;
    }

    Insert(idx, root_.get());
    return true;
}

void KdTree::Insert(const IndexVec &points, KdTreeNode *node) {
    nodes_.insert({node->id_, node});

    if (points.empty()) {
        return;
    }

    if (points.size() == 1) {
        size_++;
        node->point_idx_ = points[0];
        return;
    }

    IndexVec left, right;
    if (!FindSplitAxisAndThresh(points, node->axis_index_, node->split_thresh_, left, right)) {
        size_++;
        node->point_idx_ = points[0];
        return;
    }

    const auto create_if_not_empty = [&node, this](KdTreeNode *&new_node, const IndexVec &index) {
        if (!index.empty()) {
            new_node = new KdTreeNode;
            new_node->id_ = tree_node_id_++;
            Insert(index, new_node);
        }
    };

    create_if_not_empty(node->left_, left);
    create_if_not_empty(node->right_, right);
}

bool KdTree::FindSplitAxisAndThresh(const IndexVec &point_idx, int &axis, float &th, IndexVec &left, IndexVec &right) {
    // 计算三个轴上的散布情况，我们使用math_utils.h里的函数
    Vec3f var;
    Vec3f mean;
    math::ComputeMeanAndCovDiag(point_idx, mean, var, [this](int idx) { return cloud_[idx]; });
    int max_i, max_j;
    var.maxCoeff(&max_i, &max_j);
    axis = max_i;
    th = mean[axis];

    for (const auto &idx : point_idx) {
        if (cloud_[idx][axis] < th) {
            // 中位数可能向左取整
            left.emplace_back(idx);
        } else {
            right.emplace_back(idx);
        }
    }

    // 边界情况检查：输入的points等于同一个值，上面的判定是>=号，所以都进了右侧
    // 这种情况不需要继续展开，直接将当前节点设为叶子就行
    if (point_idx.size() > 1 && (left.empty() || right.empty())) {
        return false;
    }

    return true;
}

示例代码是一个Kd树的构建过程的C++实现，其中包含了一些关键的函数和结构体。

结构体 `KdTreeNode`

这个结构体定义了Kd树中的一个节点，包含节点的ID、点的索引、分割轴、分割阈值以及指向左右子树的指针。IsLeaf函数用于判断一个节点是否为叶子节点。

类 `KdTree`

这个类包含了构建Kd树的方法。BuildTree函数是构建Kd树的入口点，它首先清空当前的树节点，并复制点云数据到内部存储。然后，它调用Insert函数来递归地构建树。

函数 `BuildTree`

这个函数接收一个点云的智能指针cloud，如果点云为空，则返回false。它将点云数据复制到内部存储cloud_，然后清空树并重置节点ID。之后，它使用Insert函数开始构建树。

函数 `Insert`

这个函数递归地构建Kd树。如果传入的点集为空，它直接返回。如果点集中只有一个点，它将该点设置为当前节点的索引。如果点集大小大于1，它调用FindSplitAxisAndThresh函数来找到最佳的分割轴和阈值，然后递归地在左右子树上构建树。

函数 `FindSplitAxisAndThresh`

这个函数计算三个轴上的分散情况，并选择方差最大的轴作为分割轴。它还计算了该轴上的中位数作为分割阈值，并将点集分为左右两个子集。如果所有点都在分割阈值的同一侧，或者点集只有一个点，它返回false，表示不需要进一步分割。

2.k-d树k近邻查找

基于您提供的步骤，以下是KD树k近邻查找的详细算法描述：

a. 输入

KD树 ( T )，查找点 ( x )，最近邻数 ( k )。

b. 输出

k近邻集合 ( N )。

c. 初始化

设置当前节点 ( n_c ) 为根节点。
定义一个优先队列 ( PQ ) 来保存可能的最近邻点，按照它们到查找点 ( x ) 的距离排序。
初始化一个集合 ( N ) 来保存最终的k个最近邻点。

d. 递归搜索函数 ( S(n_c) )

计算距离：计算查找点 ( x ) 到当前节点 ( n_c ) 的距离 ( d(n_c, x) )。
更新优先队列：如果 ( PQ ) 的大小小于 ( k )，将 ( n_c ) 添加到 ( PQ ) 中。如果 ( PQ ) 的大小等于 ( k ) 并且 ( d(n_c, x) ) 小于 ( PQ ) 中最大距离，替换 ( PQ ) 中的最大距离点。
分割轴判断：如果 ( n_c ) 不是叶子节点，根据 ( x ) 在分割轴上的值决定搜索哪个子树。
- 选择子树：如果 ( x ) 在分割轴上的值小于 ( n_c ) 的分割值，先搜索左子树，然后搜索右子树。否则，先搜索右子树，然后搜索左子树。
- 递归搜索：对选定的子树递归调用 ( S ) 函数。
- 回溯搜索：在回溯到 ( n_c ) 后，检查另一侧子树是否有可能包含更近的点。这可以通过比较 ( x ) 到分割轴的距离与 ( PQ ) 中最大距离来判断。如果 ( x ) 到分割轴的距离小于 ( PQ ) 中最大距离，递归搜索另一侧子树，否则不对右侧子树进行展开。

e. 处理叶子节点

当 ( n_c ) 是叶子节点时，计算 ( x ) 到 ( n_c ) 代表的点的距离。
如果这个距离小于 ( PQ ) 中最大距离，将 ( n_c ) 添加到 ( PQ ) 中并更新 ( N )。

f. 完成搜索

当所有节点都被访问后，将 ( PQ ) 中的点复制到 ( N ) 中。

当k = 1即为最近邻搜索，根据上述步骤给出代码示例如下：

/// 用于记录knn结果
struct NodeAndDistance {
    NodeAndDistance(KdTreeNode* node, float dis2) : node_(node), distance2_(dis2) {}
    KdTreeNode* node_ = nullptr;
    float distance2_ = 0;  // 平方距离，用于比较

    bool operator<(const NodeAndDistance& other) const { return distance2_ < other.distance2_; }
};

bool KdTree::GetClosestPoint(const PointType &pt, std::vector<int> &closest_idx, int k) {
    if (k > size_) {
        LOG(ERROR) << "cannot set k larger than cloud size: " << k << ", " << size_;
        return false;
    }
    k_ = k;

    std::priority_queue<NodeAndDistance> knn_result;
    Knn(ToVec3f(pt), root_.get(), knn_result);

    // 排序并返回结果
    closest_idx.resize(knn_result.size());
    for (int i = closest_idx.size() - 1; i >= 0; --i) {
        // 倒序插入
        closest_idx[i] = knn_result.top().node_->point_idx_;
        knn_result.pop();
    }
    return true;
}

void KdTree::Knn(const Vec3f &pt, KdTreeNode *node, std::priority_queue<NodeAndDistance> &knn_result) const {
    if (node->IsLeaf()) {
        // 如果是叶子，检查叶子是否能插入
        ComputeDisForLeaf(pt, node, knn_result);
        return;
    }

    // 看pt落在左还是右，优先搜索pt所在的子树
    // 然后再看另一侧子树是否需要搜索
    KdTreeNode *this_side, *that_side;
    if (pt[node->axis_index_] < node->split_thresh_) {
        this_side = node->left_;
        that_side = node->right_;
    } else {
        this_side = node->right_;
        that_side = node->left_;
    }

    Knn(pt, this_side, knn_result);
    if (NeedExpand(pt, node, knn_result)) {  // 注意这里是跟自己比
        Knn(pt, that_side, knn_result);
    }
}

void KdTree::ComputeDisForLeaf(const Vec3f &pt, KdTreeNode *node,
                               std::priority_queue<NodeAndDistance> &knn_result) const {
    // 比较与结果队列的差异，如果优于最远距离，则插入
    float dis2 = Dis2(pt, cloud_[node->point_idx_]);
    if (knn_result.size() < k_) {
        // results 不足k
        knn_result.emplace(node, dis2);
    } else {
        // results等于k，比较current与max_dis_iter之间的差异
        if (dis2 < knn_result.top().distance2_) {
            knn_result.emplace(node, dis2);
            knn_result.pop();
        }
    }
}

bool KdTree::NeedExpand(const Vec3f &pt, KdTreeNode *node, std::priority_queue<NodeAndDistance> &knn_result) const {
    if (knn_result.size() < k_) {
        return true;
    }

    if (approximate_) {
        float d = pt[node->axis_index_] - node->split_thresh_;
        if ((d * d) < knn_result.top().distance2_ * alpha_) {
            return true;
        } else {
            return false;
        }
    } else {
        // 检测切面距离，看是否有比现在更小的
        float d = pt[node->axis_index_] - node->split_thresh_;
        if ((d * d) < knn_result.top().distance2_) {
            return true;
        } else {
            return false;
        }
    }
}