三维点云处理-点云的近邻搜索

已于 2024-04-22 12:03:18 修改
阅读量1.1k 收藏 20
点赞数 22
分类专栏： 三维点云处理 文章标签： 算法
于 2024-04-22 12:02:24 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_49401384/article/details/138069838
版权

  常见的点云邻域查找方法包括：K-NN 查找和Radius-NN 查找。两者的区别主要在于：

  • K-NN查找是给定待查找点 q，寻找点集距离 q 点最近的 k 个点；
  • Radius-NN查找是给定待查找点 q，以及查找半径 r，寻找 q 点附近半径为 r 距离内的所有点；

image.png

1. KD-Tree的近邻查找

  KD-Tree 近邻查找的基本思想：先确认待查找数据点所在的子节点后，根据目标不断向相邻子节点扩展，直到找到满足要求的近邻点数据后结束查找。而且KD-Tree 本身是一个带有约束条件的二分查找树，不需要计算所有数据点和待查找点之间的距离，可以提高查找效率。

1.1 K-NN查找

  查找方法：给定待查找点 q，寻找点集距离 q 点最近的 k 个点。
  查找流程：

  1. 从根节点开始，查找 q 点所在的叶子节点；
  2. 判断叶子节点内的点是否满足查找目标，若满足，则将查找到的 k 个点记录下来；否则，继续扩大查找范围，寻找相邻的叶子节点；
  3. 直到查找到满足要求的 k 个点后结束。

image.png
  对于上面流程中的第2点，如何扩大查找范围呢？
image.png
  这里用到了KD-Tree当前子节点的切分值以及查找过程中的最远查找距离。最远查找距离(w)表示已经查找到的 q 的近邻点中距离第 k 大的值。对于非叶子节点：

  1. 如果 待查点 q 在该维度上的值小于/大于 当前节点的切分值，则需要到当前节点的左节点/右节点继续查找；
  2. 进一步，如果 待查点 q 到子节点切分值的距离小于最远查找距离，则说明相邻子节点中可能存在距离 待查点 q 更近的点，需要查找该节点的另一部分子树；

image.png

1.2 Radius-NN查找

  相比于前面的 K-NN 查找，Radius-NN取消了查找数量 K 的约束，同时对查找过程中的最远查找距离(w)进行了约束，因此两种方法的查找过程类似。
image.png

1.3 代码练习

#pragma once

#include "KD_Tree.h"
#include "KNN_result.h"

class KDTreeAVLNearestNeighbors {
public:
    bool set_data(const Eigen::MatrixXd& input_matrix, int leaf_size);
    bool KNN_search_number(const Eigen::MatrixXd& key, KNNResultNumber& knn_result);
    bool KNN_search_radius(const Eigen::MatrixXd& key, KNNResultRadius& knn_result);

private:
    bool KNN_search_number(const std::shared_ptr<PCDTREE::KDTreeNode>& root,
                           const Eigen::MatrixXd& key, KNNResultNumber& knn_result);

    bool KNN_search_radius(const std::shared_ptr<PCDTREE::KDTreeNode>& root,
                           const Eigen::MatrixXd& key, KNNResultRadius& knn_result);

private:
    Eigen::MatrixXd _input_matrix;
    PCDTREE::KDTree _kd_tree;
};

#include "nearest_neighbors.h"

// kd-tree的近邻查找部分
bool KDTreeAVLNearestNeighbors::set_data(const Eigen::MatrixXd& input_matrix, int leaf_size){
    _kd_tree.input(input_matrix);
    _input_matrix = input_matrix;
    return _kd_tree.create_kd_tree(leaf_size);  
}

bool KDTreeAVLNearestNeighbors::KNN_search_number(const Eigen::MatrixXd& key, KNNResultNumber& knn_result){
    return KNN_search_number(_kd_tree.get_root(), key, knn_result);
}

bool KDTreeAVLNearestNeighbors::KNN_search_radius(const Eigen::MatrixXd& key, KNNResultRadius& knn_result){
    return KNN_search_radius(_kd_tree.get_root(), key, knn_result);
}

bool KDTreeAVLNearestNeighbors::KNN_search_number(const std::shared_ptr<PCDTREE::KDTreeNode>& root,
                                                  const Eigen::MatrixXd& key, KNNResultNumber& knn_result){
    // 判断当前节点是否为空
    if(root == nullptr){
        return false;
    }
    // 判断当前节点是否为叶子节点
    if(root->is_leaf){
        // 遍历判断每个点到key的距离
        for (size_t i = 0; i < root->value_indices.size(); ++i){
            Eigen::MatrixXd vdiff = key.col(0) - _input_matrix.col(root->value_indices[i]);
            double diff = vdiff.norm();
            knn_result.add_result(diff, root->value_indices[i]);
        }
        return true;
    }
    // 正常判断该节点的左右子节点
    if(key(root->axis, 0) <= root->key){
        KNN_search_number(root->left, key, knn_result);
        // 判断最远查找距离是否超过root分割线
        if(abs(key(root->axis, 0) - root->key) < knn_result.worst_distance()){
            KNN_search_number(root->right, key, knn_result);
        }
    }else{
        KNN_search_number(root->right, key, knn_result);
        if(abs(key(root->axis, 0) - root->key) < knn_result.worst_distance()){
            KNN_search_number(root->left, key, knn_result);
        }
    }
    return true;
}

bool KDTreeAVLNearestNeighbors::KNN_search_radius(const std::shared_ptr<PCDTREE::KDTreeNode>& root,
                                                  const Eigen::MatrixXd& key, KNNResultRadius& knn_result){
    // 判断当前节点是否为空
    if(root == nullptr){
        return false;
    }
    // 判断当前节点是否为叶子节点
    if(root->is_leaf){
        // 遍历判断每个点到key的距离
        for(size_t i = 0; i < root->value_indices.size(); ++i){
            Eigen::Vector3d vdiff = key.col(0) - _input_matrix.col(root->value_indices[i]);
            double diff = vdiff.norm();
            knn_result.add_result(diff, root->value_indices[i]);
        }
        return true;
    }
    // 正常判断该节点的左右子节点
    if(key(root->axis, 0) < root->key) {
        KNN_search_radius(root->left, key, knn_result);
        // 判断最远查找距离是否超过root分割线
        if(abs((key(root->axis) - root->key)) < knn_result.worst_distance()){
            KNN_search_radius(root->right, key, knn_result);
        }
    }else{
        KNN_search_radius(root->right, key, knn_result);
        if(abs(key(root->axis, 0) - root->key) < knn_result.worst_distance()){
            KNN_search_radius(root->left, key, knn_result);
        }
    }
    return true;
}

2. Octree的近邻查找

  Octree 近邻查找的基本思想：先确认待查找数据点所在的子节点后，根据目标不断向相邻子节点扩展，直到找到满足要求的近邻点数据后结束查找。因每个节点都是一个小的立方体，而待查找点 q 邻域是一个球体，扩展搜索也就涉及到球和立方体之间位置关系的判断，如不相交、部分相交、包含等。
image.png

2.1 位置关系判断

  球和立方体的空间位置关系主要可以分为包含、不相交、部分相交等情况。

2.1.1 球在立方体内（包含）

image.png
  这种情况最直观的判断条件就是：立方体中心点到球邻域的最远端的距离有没有超出立方体的边长的一半。从图上看就是 立方体中心点(绿色点)到球体中心(红色点)的距离 加上 球体半径 的长度 是否小于 立方体边长的一半。

// 判断 待查找点邻域 是否被包含于 立方体中
// 输入：待查找点及其邻域半径，需要比较的立方体
bool Octree::inside(const Eigen::Vector3d& query, 
                    const std::shared_ptr<Octant>& octant,
                    double radius){
    Eigen::Vector3d query_offset = (query - octant->center).cwiseAbs();
    Eigen::Vector3d possible_space = query_offset + Eigen::Vector3d(radius, radius, radius);
    if (possible_space(0) < octant->extent &&
        possible_space(1) < octant->extent &&
        possible_space(2) < octant->extent) {
        return true;
    }else{
        return false;
    }
}

2.1.2 球和立方体相交的情况

  主要分为完全不相交（球在立方体外）、部分相交（球和立方体的面相交）、部分相交（球和立方体的顶点/边相交）。
image.png

  1. 球在立方体外
    image.png
      这种情况最直观的判断条件就是：任意维度上 查询点到立方体中心的距离 超过 球邻域半径+立方体边长的一半。

  2. 球和立方体的面相交
      如果我们知道球和立方体之间的距离不是特别远，即排除了两者完全不相交的情况，那么如何判断两者的相交程度呢？
    image.png
      这种情况最直观的判断条件就是：任意两个维度及以上，查询点到立方体中心的距离 小于 立方体边长的一半。（=2 表示球与立方体相切，=3 表示相交）

  3. 球和立方体的顶点/边相交
    image.png
    这种情况最直观的判断条件就是：判断 查询点 到立方体端点的距离 是否小于 球邻域半径。

bool Octree::overlap(const Eigen::Vector3d& query,
                     const std::shared_ptr<Octant>& octant,
                     double radius){
    Eigen::Vector3d query_offset = (query - octant->center).cwiseAbs();
    double max_dist = octant->extent + radius;
    // case 1: completely outside
    // 球和体距离太远：任意维度 查询点到体中心的距离 超过 球邻域半径+体半径
    for (int i = 0; i < 3; ++i){
        if(query_offset(i) > max_dist){
            return false;
        }
    }

    // case 2: contacting the face of the octant  
    // 球和面的接触： 任意两个维度 查询点到体中心的距离 小于 体半径
    Eigen::Vector3d is_contact_face;
    for (int i = 0; i < 3; ++i){
        is_contact_face(i) = query_offset(i) < octant->extent ? 1 : 0;
    }
    if (is_contact_face.sum() >= 2) {   // =2 表示query ball与立方体相贴，=3 表示相交
        return true;
    }

    // case 3: query ball is contacting the edge or corner of octant
    // 球和顶点/边的接触 判断 查询点到体端点的距离 是否小于 球邻域半径
    double x_diff = std::max(query_offset(0) - octant->extent, 0.);
    double y_diff = std::max(query_offset(1) - octant->extent, 0.);
    double z_diff = std::max(query_offset(2) - octant->extent, 0.);

    return x_diff * x_diff + y_diff * y_diff + z_diff * z_diff < radius * radius;
}

2.2 K-NN查找

  查找方法：给定待查找点 q，寻找点集距离 q 点最近的 k 个点。
  查找流程：

  1. 从根节点开始，查找 q 点所在的叶子节点；
  2. 判断叶子节点内的点是否满足查找目标，若满足，则将查找到的 k 个点记录下来；否则，继续扩大查找范围，寻找相邻的叶子节点；
  3. 直到查找到满足要求的 k 个点后结束。

image.png
  Octree扩大查找判断的方式：如果查找点所在的立方体无法满足查找目标，那么就会遍历查找相邻的7个子节点；如果能够找到满足条件的近邻点，则结束查找，否则继续查找上一级节点，直到找到可以包含 查找点球邻域 的立方体为止。
image.png

2.3 Radius-NN查找

  相比于前面的 K-NN 查找，Radius-NN取消了查找数量 K 的约束，同时对查找过程中的最远查找距离(w)进行了约束，因此其查找核心在于如何找到包含 查找点球邻域 的最小立方体。
image.png

2.4 代码练习

#pragma once

#include "Octree.h"
#include "KNN_result.h"

class OctreeAVLNearestNeighbors {
public:
    bool set_data(const Eigen::MatrixXd& input_matrix, int leaf_size, double min_length);
    bool KNN_search_number(const Eigen::MatrixXd& key, KNNResultNumber& knn_result);
    bool KNN_search_radius(const Eigen::MatrixXd& key, KNNResultRadius& knn_result);

private:
    bool KNN_search_number(const std::shared_ptr<PCDTREE::Octant>& root,
                           const Eigen::MatrixXd& key, KNNResultNumber& knn_result);
    bool KNN_search_radius(const std::shared_ptr<PCDTREE::Octant>& root,
                           const Eigen::MatrixXd& key, KNNResultRadius& knn_result);
private:
    Eigen::MatrixXd _input_matrix;
    PCDTREE::Octree _octree;
};

#include "nearest_neighbors.h"

bool OctreeAVLNearestNeighbors::set_data(const Eigen::MatrixXd& input_matrix, int leaf_size, double min_length){
    _input_matrix = input_maxtix;
    _octree.input(_input_matrix);
    return _octree.build(leaf_size, min_length);
}

bool OctreeAVLNearestNeighbors::KNN_search_number(const Eigen::MatrixXd& key, KNNResultNumber& knn_result){
    return KNN_search_number(_octree.get_root(), key, knn_result);
}

bool OctreeAVLNearestNeighbors::KNN_search_radius(const Eigen::MatrixXd& key, KNNResultRadius& knn_result){
    return KNN_search_radius(_octree.get_root(), key, knn_result);
}

bool OctreeAVLNearestNeighbors::KNN_search_number(
                            const std::shared_ptr<Octant>& root,
                            const Eigen::MatrixXd& key,
                            KNNResultNumber& knn_result){
    // 判断当前节点是否为空
    if(root == nullptr){
        return false;
    }
    // 判断当前是否是叶子节点
    if(root->is_leaf && root->value_indices.size() > 0){
        // 将key与叶子节点内的所有点进行比较
        for(size_t i = 0; i < root->value_indices.size(); ++i){
            Eigen::Vector3d vdiff = key.col(0) - _input_matrix.col(root->value_indices[i]);
            double diff = vdiff.norm();
            knn_result.add_result(diff, root->value_indices[i]);
        }
        return Octree::inside(key.col(0), root, knn_result.worst_distance());
    }

    // 计算key所在的子节点
    u_char morton_mode = 0;
    if(key.col(0)(0) > root->center(0)) morton_mode |= 1;
    if(key.col(0)(1) > root->center(1)) morton_mode |= 2;
    if(key.col(0)(2) > root->center(2)) morton_mode |= 4;

    if(KNN_search_number(root->children[int(morton_mode)], key, knn_result){
        return true;
    }
    // 检查其他子节点
    for(int i = 0; i < 8; ++i){
        // 遇到key所在的子节点或者空的子节点，跳过
        if(i == int(morton_mode) || root->children[i] == nullptr){
            continue;
        }
        // 遇到与key邻域不相交的子节点，跳过
        if(!Octree::overlap(key.col(0), root->children[i], knn_result.worst_distance())){
            continue;
        }
        // 正常查找key的邻域点
        if(KNN_search_number(root->children[i], key, knn_result){
            return true;
        }
    }
    // 检查搜索是否可以终止，即是否找到了 包含 key所在球邻域 的立方体
    return Octree::inside(key.col(0), root, knn_result.worst_distance());
}

bool OctreeAVLNearestNeighbors::KNN_search_radius(
                            const std::shared_ptr<Octant>& root,
                            const Eigen::MatrixXd& key,
                            KNNResultNumber& knn_result){
    // 判断当前节点是否为空
    if(root == nullptr){
        return false;
    }
    // 判断当前节点是否为叶子节点
    if(root->is_leaf && root->value_indices.size() > 0){
        // 将key与叶子中的所有点比较
        for(size_t i = 0; i <　root->value_indices.size(); ++i){
            Eigen::Vector3d vdiff = key.col(0) - _input_matrix.col(root->value_indices[i]);
            double diff = vdiff.norm();
            knn_result.add_result(diff, root->value_indices[i]);
        }
        return Octree::inside(key.col(0), root, knn_result.worst_distance());
    }
    // 计算key所在的子节点并查找
    u_char morton_mode = 0;
    if(key.col(0)(0) > root->center(0)) morton_mode |= 1;
    if(key.col(0)(1) > root->center(1)) morton_mode |= 2;
    if(key.col(0)(2) > root->center(2)) morton_mode |= 4;

    if(KNN_search_radius(root->children[int(morton_mode)], key, knn_result.worst_distance()){
        return true;
    }
    // 遍历判断其他子节点
    for(int i = 0; i < 8; ++i){
        // 遇到key所在子节点或者当前子节点为空时，跳过
        if(i == int(morton_mode) || root->children[i] == nullptr){
            continue;
        }
        // 遇到 与key查找邻域不相交 的子节点时，跳过
        if(!Octree::overlap(key.col(0), root->children[i], knn_result.worst_distance())){
            continue;
        }
        // 正常处理其他节点
        if(KNN_search_radius(root->children[i], key, knn_result.worst_distance())){
            return true;
        }
    }
    // 检查搜索是否可以终止，即是否找到了 包含 key所在球邻域 的立方体
    return Octree::inside(key.col(0), root, knn_result.worst_distance());
}

声明：以上公式和图片来自课程上的PPT部分，部分参考借鉴了其他博主，仅作为学习、交流使用。

参考链接：
  1. analyze_pointcloud/Nearest_Neighbors at master · QinZiwen/analyze_pointcloud
  2. 三维点云处理-深蓝学院
  3. 机器学习算法（二十五）：KD树详解及KD树最近邻算法
小白要加油学习哇
关注
  • 22
    点赞
  • 踩
  • 20
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
【C++PCL】点云处理Kd-tree邻域搜索
weixin_45196569的博客
06-24 163
int k,为给定的查询点搜索k个最近邻。该方法不会对输入索引(i.e., index >= cloud.size () || index < 0), 做任何边界检查，并假设数据有效(即有限)。point：一个给定的有效(即有限)查询点k：要搜索的邻居数量k_indices：相邻点的合成指数(必须被预先调整为k!：到相邻点的平方距离(必须预先调整为k!
matlab 点云的圆柱形邻域搜索（详细过程版）
点云侠的博客
10-04 1090
点云圆柱形邻域搜索的matlab代码实现
（3）点云数据处理学习——KD树近邻搜索
chencaw的专栏
11-29 1160
dd
k-近邻算法的原理和代码实现
大胡子的博客
07-17 7154
 一、k-近邻算法的基本原理 对于未知类别属性数据集中的点： （1）计算已知类别数据集中的每个点与当前点之间的距离； 使用欧式距离公式，计算两个向量点xA和xB之间的距离公式如下：                  （2）按照距离递增次序排序； （3）选取与当前点距离最小的k个点； （4）确定前k个点所在类别的出现频率； （5）返回前k个点出现频率最高的类别作为当前点的预测分类。 ...
Matlab三维离散点云的邻域点搜索（柱状邻域、球状邻域及KNN）
10-12
利用matlab构建kd树，通过KNN、球状邻域与柱状邻域三种搜索方式高效获取当前点的三种邻域点。压缩包中包括主函数nbselect、demo、示例数据与结果图。
三维激光点云邻域快速搜索
04-04
采用python语言编程实现，激光点云K邻域、球邻域和柱形邻域，三种领域的快速搜索，并附有实例
点云构建kd树，k邻域检索
harden1013的博客
07-25 975
1.介绍 kd树（k-dimensional tree） 主要思路：利用中位数作为二分点（根节点），不断下分至子节点没有点 2.代码 /*依赖库文件*/ #include <pcl/point_cloud.h> #include <pcl/kdtree/kdtree_flann.h> int main() { /*待处理的点云*/ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointC
对三维点云求出梯度方向并画图
05-11
在本场景中，我们关注的是如何利用MATLAB来处理三维点云数据，特别是计算点云的数字表面模型（DSM）以及绘制梯度方向图。 首先，DSM（Digital Surface Model）是一种表示地表特征的数字模型，它包含了地表所有可见...
三维点云处理kdtree,octree，python程序
04-27
在三维点云处理领域，kd树（kd-Trees）和八叉树（Octrees）是两种常见的数据结构，用于高效地存储和检索大量空间数据。它们在计算机图形学、机器学习以及遥感等领域有着广泛的应用。本文将详细介绍这两种数据结构，...
k-近邻算法（kNN）
09-20
采用测量不同特征值之间的距离方法进行分类 工作原理:存在一个样本数据集合(训练样本集),并且样本集中每个数据都存在标签,即我们知道样本集中每一数据与所属分类的对应关系。 　　输入没有标签的新数据后,将新数据的每个特征与样本集中数据对应的特征进行比较,然后算法提取样本集中特征醉相思数据(最近邻)的分类标签。
基于PCL的三维点云模型表面主曲率估计的具体实现
04-27
PCL（Point Cloud Library）是一个强大的开源库，专门用于处理和分析三维点云数据。本篇文章将深入探讨如何利用PCL进行三维点云模型表面主曲率的估计，这对于理解点云表面的几何特性、识别物体边缘以及在机器视觉和...
三维点云精简的均匀网格法&不均匀网格法，matlab实现，带例程
03-05
总结来说，这个压缩包文件提供了关于三维点云精简的实用资源，涵盖了均匀和不均匀网格法的MATLAB实现。通过学习和理解这些例程，读者不仅可以掌握点云处理的基本技术，还能进一步探索和优化算法，以满足特定应用场景...
三维点云课程（二）——最邻近问题
HUASHUDEYANJING的博客
07-12 1195
点云最邻近问题难点：1. 不规则 2. 三维 3. 数据量大算法核心点： 1. 分隔空间 2. 跳过空间 3. 停止搜索左放小，大放右 多维：找到worst distance是最重要的，其是动态的
三维点云处理之最近邻问题
LDST_CSDN的博客
04-26 1894
本系列文章是关于三维点云处理的常用算法，深入剖析pcl库中相关算法的实现原理，并以非调库的方式实现相应的demo。
点云处理算法整理（超详细教程）十大点云数据处理技术梳理
热门推荐
weixin_58797039的博客
12-14 2万+
点云配准的概念类比于二维图像中的配准，只不过二维图像配准获取得到的是x，y，alpha，beta等放射变化参数，二三维点云配准可以模拟三维点云的移动和对齐，也就是会获得一个旋转矩阵和一个平移向量，通常表达为一个4×3的矩阵，其中3×3是旋转矩阵，1*3是平移向量。如果要对一个三维点云进行描述，光有点云的位置是不够的，常常需要计算一些额外的参数，比如法线方向、曲率、文理特征等等。主要包含以下几方面：点云压缩，点云索引（KDtree、Octree），点云LOD（金字塔），海量点云的渲染。
三维点云处理(4)——KNN
weixin_44554973的博客
06-11 2926
Binary Search TreeNearest Neighbor (NN) ProblemK-NNFixed Radius-NNCore IdeasBST Nearest Neighbor (NN) Problem K-NN Given a set of points SSS in a space MMM, a query point q∈Mq{\in}Mq∈M, find the kkk closest points in SSS Fixed Radius-NN Given a set of p
点云 K 临近查找算法：kd 树
大家好~我是SP_FA~
07-09 346
kd 树是一个二叉树，用于高效的查找某个点的 k 临近点，它的每一个节点记载了 特征坐标，切分轴，指向左右子树的指针。
【排序算法】插入排序（希尔排序）
最新发布
2301_80555259的博客
07-07 700
直接插入排序是一种简单的插入排序法，其核心思想是对一个已经有序的序列插入一个数据，该数据依次比较有序序列中的值，直到插入到合适的位置。在我们玩扑克牌整理牌序的时候，用到的就是直接插入排序的思想。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值