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

一、问题引出

最近邻问题：假设有两个点云集合， $\chi _{1} = \left \{ x_{1},\cdots x_{n} \right \}$ ， $\chi _{2} = \left \{ x_{1},\cdots x_{n} \right \}$ ，求点云集合 $\chi _{2}$ 中某个点，在点云集合 $\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 不足kknn_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;}}
}