三维点云处理-KDTree和Octree

本文介绍了点云数据处理中的核心问题,包括如何通过KD-Tree和Octree建立离散点间的拓扑关系,以及这两种数据结构在高维空间中的应用,重点讲解了KD-Tree的构建原理、递归划分过程和Octree的构建步骤,以及它们在近邻搜索中的作用。
摘要由CSDN通过智能技术生成

  点云数据主要是表征目标表面的海量点集合,并不具备传统实体网格数据的集合拓扑信息。因此,如何建立离散点间的拓扑关系,实现基于邻域关系的快速查找也是点云数据处理中比较核心的问题。对于一维数据来说,典型的树形存储结构如Binary Search Tree(BST),特点在于:对于树中的每个节点,其左子树中的所有节点的值都小于该节点的值,而右子树中的所有节点的值都大于该节点的值,即中序遍历是有序的。这种结构在执行搜索操作时效率较高,在平均情况下,如果树保持相对平衡,BST 的时间复杂度为 O(log n)。
  对于高维数据,同样可以采用这种自顶向下逐级划分数据的空间索引结构,如三维数据中比较常见的KD-Tree和Octree。本节主要是学习理解KD-Tree/Octree的构建以及近邻搜索原理,仅作为学习记录!

1. KD-Tree

  k-d tree,也称 k 维树,是对数据点在 k 维空间(如二维-xy,三维-xyz,K维-xyz…k)中划分的一种数据结构,主要应用于多维空间数据的搜索(如最近邻搜索、范围搜索)。k-d tree 的每一级在指定维度上(如 x 轴)分开了所有的子节点,在该维度上,小于根节点的部分划分为左子树,大于根节点的部分划分到右子树,所以本质上也是一种带有约束条件的二分查找树。

在这里插入图片描述

1.1 KD-Tree的构建

  基本思想:采用分而治之的思想,根据数据特征选择一个合适的维度作为切分维度,并根据该维度的中位数作为切分值,进而根据切分维度和切分值,将数据点分割成两个子集,左子集包含小于等于切分值的数据点,右子集包含大于切分值的数据点,直到所有数据都切分完成。
  经典的构造KD-Tree的方式如下:

  1. 给定初始化坐标轴作为根节点分割超平面的法向量。如3-d tree,根节点通常选取为 X 轴;
  2. 随着树的深度的增加,循环的选取坐标轴,作为分割超平面的法向量,如3-d tree,根节点的左右子树选取 Y 轴,左右子树的下一级选择 Z 轴,再下一级选择 X轴,依次类推;
  3. 每次在进行当前节点的左右子树切分时,可以选择该维度上数据的中位数作为切分点。
    在这里插入图片描述
      如上图所示,每个非叶子节点都可以想象成一个分割超平面,用垂直于坐标轴的超平面将空间分成两个部分,不断对数据进行递归划分,直到所有数据都切分完成。对于n个实例的k维数据来说,KD-Tree构建的时间复杂度在O(n logn) - O(n log n log n)不等;

1.2 代码练习

  这部分依然以三维数据作为输入进行代码练习。切分点使用当前维度上数据的均值来代替中值,切分超平面采用xyz轴轮转的形式。主要流程如下:

  • 输入部分:原始三维数据,叶子节点最少点数
  • 输出部分:kd-tree的根节点
  • 处理步骤:
    1. 输入三维数据,根据切分维度选择切分点进行左右子树切分;
    2. 采用循环选取坐标轴的方式,递归构建左右子树;
    3. 不断递归划分直到所有数据都切分完成。
#pragma once

#include <memory>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Core>

// 定义KD-Tree的基础节点
typedef struct KDTreeNode{
    int axis;	// 切分轴
    double key;	// 切分点
    bool is_leaf;	// 是否为叶子节点

    std::vector<int> value_indices;		// 当前节点内存放的数据索引
    std::shared_ptr<KDTreeNode> left;	// 左节点
    std::shared_ptr<KDTreeNode> right;	// 右节点
    KDTreeNode(): axis(-1), key(0), is_leaf(false) {}
    KDTreeNode(int axis_): axis(axis_), key(0), is_leaf(false) {}

} KDTreeNode;

// 定义KD-Tree
// 属性:根节点、点云数据
// 方法:点云数据初始化、构建KD-Tree(对外接口)、递归构建KD-Tree(对内接口)、获取根节点
// 辅助方法:获取数据当前节点的切分信息(key,value_indices,左子树索引、右子树索引),获取切分轴
class KDTree{
public:
    void set_data(const Eigen::MatrixXd& input_data);	// 输入数据
    bool create_kd_tree(int leaf_size);			// 构建KD-Tree(外部接口)
    std::shared_ptr<KDTreeNode> get_root();	// 获取KD-Tree的根节点(外部接口)
private:
    // 递归构建KD-Tree
    // 输入:需要构建的节点、原始数据、待构建的点索引、切分轴、叶子节点最少点数
    bool create_kd_tree_recursive(std::shared_ptr<KDTreeNode>& root,
                                  const Eigen::MatrixXd& values,
                                  std::vector<int>& value_indives,
                                  int axis,
                                  int leaf_size);
    // 获取构建当前节点需要的分割数据
    // 输入:原始数据、待构建的点索引、切分轴
    // 输出:切分值、切分值对应的索引、切分后的左节点索引、切分后的右节点索引
    bool get_median_key_and_left_right_value_indices_maxmin(
                                  const Eigen::MatrixXd& values,
                                  const std::vector<int> value_indives,
                                  int axis,
                                  double& median_key,
                                  std::vector<int>& median_value_indices,
                                  std::vector<int>& left_value_indices,
                                  std::vector<int>& right_value_indices);
    int get_next_axis(int axis, int dim);
private:
    Eigen::MatirxXd _data;
    std::shared_ptr<KDTreeNode> _root;
};
#include "KD_Tree.h"

void KDTree::set_data(const Eigen::MatrixXd& input_data){
    _data = input_data;
}

std::shared_ptr<KDTreeNode> KDTree::get_root(){
    return _root;
}

bool KDTree::create_kd_tree(int leaf_size){
    // 构建数据的索引列表
    std::vector<int> value_indices;
    for(size_t i = 0; i < _data.cols(); ++i){
        value_indices.emplace_back(i);
    }
    return create_kd_tree_recursive(_root, _data, value_indices, 0, leaf_size);
}

bool KDTree::create_kd_tree_recursive(std::shared_ptr<KDTreeNode>& root,
                                      const Eigen::MatrixXd& values,
                                      std::vector<int> value_indives,
                                      int axis,
                                      int leaf_size){
    // 判断当前节点是否为空
    if(root == nullptr){
        root.reset(new KDTreeNode(axis));
    }
    // 判断是否需要切分
    if(value_indives.size() > leaf_size){
        double median_key;
        std::vector<int> median_value_indices, left_value_indices, right_values_indices;
        get_median_key_and_left_right_value_indices_maxmin(values, value_indives, axis, 
            median_key, median_value_indices, left_value_indices, right_values_indices);
        // 构建当前节点
        root->is_leaf = false;
        root->key = median_key;
        root->value_indices = median_value_indices;
        // 构建左右子树
        int next_axis = get_next_axis(axis, values.rows());
        create_kd_tree_recursive(root->left, values, left_value_indices, next_axis, leaf_size);
        create_kd_tree_recursive(root->right, values, right_value_indices, next_axis, leaf_size);
    }else{
        root->is_leaf = true;
        root->value_indices = value_indives;        
    }
    return true;
}

bool KDTree::get_median_key_and_left_right_value_indices_maxmin(
                                        const Eigen::MatrixXd& values,
                                        const std::vector<int> value_indives,
                                        int axis,
                                        double& median_key,
                                        std::vector<int>& median_value_indices,
                                        std::vector<int>& left_value_indices,
                                        std::vector<int>& right_value_indices){
    // 获取均值
    Eigen::VectorXd tmp_values(value_indives.size());
    for(int i = 0; i < value_indives.size(); ++i){
        tmp_values(i) = values(axis, value_indives[i]);
    }
    double max = tmp_values.maxCoeff();
    double min = tmp_values.minCoeff();
    if(max > min){
        median_key = min + (max - min) / 2;
        // 遍历所有点划分索引区分
        for(int i = 0; i < value_indives.size(); ++i){
            if(tmp_values(i) == median_key){
                median_value_indices.emplace_back(value_indives[i]);
            }else if(tmp_values(i) < median_key){
                left_value_indices.emplace_back(value_indives[i]);
            }else{
                right_value_indices.emplace_back(value_indives[i]);
            }
        }        
    }else{
        median_key = min;
        int median_idx = floor(value_indives.size() / 2);
        median_value_indices = std::vector<int>(
            value_indices.begin() + median_idx, value_indices.begin() + median_idx + 1);
        left_value_indices = std::vector<int>(
            value_indices.begin(), value_indices.begin() + median_idx);
        right_value_indices = std::vector<int>(
            value_indices.begin() + median_idx + 1, value_indices.end());
    }
    return true;
 }

int KDTree::get_next_axis(int axis, int dim){
    if(axis >= dim -1){
        return 0;
    }else{
        return axis + 1;
    }
}

1.3 代码测试

#inlcude "KD_Tree.h"

int main(){
    Eigen::MatrixXd data(3, 4);
    data << 1, 2, 3, 4,
            1, 2, 1, 2,
            0, 1, 2, 3;
    
    KDTree kd_tree;
    kd_tree.input(data);
    kd_tree.create_kd_tree(1);

    return 0;
}

2. Octree

  八叉树结构是 Hunter 博士于 1978 年首次提出的一种数据模型,通过循环递归的划分方法对大小为2n x 2n x 2n 的三维空间的几何对象进行划分,从而构成一个具有根节点的方向图。如下图所示:
image.png

2.1 Octree的构建

  基本思想:基于设定的最短分割长度或最少数据约束,以数据的最大尺寸构建根节点,递归地将数据切分到八个子立方体中,直到所有数据都切分完成。
Octree的构建原理及一般步骤如下:

  1. 设定递归终止条件:首先,需要设定子节点的最短分割长度和最少数量约束,也就是细分过程的终止条件;
  2. 确定初始立方体:找出场景中的最大尺寸,并以此尺寸建立一个初始的立方体(根节点);
  3. 分配单位元素:将场景中的单位元元素(如点云数据中的点)丢入能被包含且没有子节点的立方体中;
  4. 递归细分:如果当前立方体没有达到递归终止条件,就继续细分;
  5. 检查细分条件:在细分过程中,如果发现某个子立方体所分配到的元素数量小于leaf_size或者立方体尺寸小于最短分割长度,则停止细分;
  6. 重复细分过程:我们继续对满足细分条件的子立方体进行上述的细分过程,直到达到设定的最大递归深度;

image.png

2.2 代码练习

  这部分依然以三维数据作为输入进行代码练习。Octree构建的主要流程如下:

  • 输入部分:原始三维数据,叶子节点最少点数,最短分割长度;
  • 输出部分:octree的根节点
  • 处理步骤:
    1. 输入三维数据,根据数据的包围框及数据的中心点进行立方体的细分;
    2. 检查细分条件,若满足停止切分;若不满足则继续进行子节点立方体的细分;
    3. 不断细分直到所有数据都切分完成。
#pragma once

#include <memory>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>

// 定义Octree的基础节点
typedef struct Octant{
    Eigen::Vector3d center;
    double extent;
    std::vector<u_int> value_indices;
    bool is_leaf;
    std::shared_ptr<Octant> children[8];

    Octant(){}
    Octant(const Eigen::Vector3d& center_, double extent_, const std::vector<u_int> value_indices_, bool is_leaf_)
    : center(center_), extent(extent_), value_indices(value_indices_), is_leaf(is_leaf_){}
}Octant;

// 定义KD-Tree
// 属性:根节点、点云数据、叶子节点最少点数、叶子节点最短分割长度
// 方法:点云数据初始化、构建Octree(对外接口)、递归构建Octree(对内接口)
class Octree{
public:
    void set_data(const Eigen::MatrixXd& input_data);
    bool build(int leaf_size, double min_length);
    std::shared_ptr<Octant> get_root();
private:
    // 递归构建子节点
    // 输入:需要构建的节点、点云数据、待分配的数据索引、立方体中心,立方体扩展长度
    build build_octree_recursive(std::shared_ptr<Octant>& root,
                                 const Eigen::MatirxXd& values,
                                 const std::vector<u_int>& value_indices,
                                 const Eigen::Vector3d& center,
                                 double extent);
private:
    Eigen::MatrixXd _data;
    std::shared_ptr<Octant> _root;

    // 子节点细分终止条件
    int _leaf_size;
    double _min_extent;
};
#include "Octree.h"

#include <map>

void Octree::set_data(const Eigen::MatrixXd& input_data){
    _data = input_data;
}

std::shared_ptr<Octant> Octree::get_root(){
    return _root;
}

bool Octree::build(int leaf_size, double min_length){
    _leaf_size = leaf_size;
    _min_extent = min_length / 2;

    // 计算立方体中心点
    Eigen::Vector3d max_value = _data.rowwise().maxCoeff();
    Eigen::Vector3d min_value = _data.rowwise().minCoeff();
    Eigen::Vector3d center = min_value + (max_value - min_value) / 2;
    double extent = (max_value - min_value).maxCoeff() / 2;
    // 获取需要细分的数据索引
    std::vector<u_int> value_indices;
    for(size_t i = 0; i < _data.cols(); ++i){
        value_indices.emplace_back(i);
    }
    // 递归构建Octree
    return build_octree_recursive(_root, _data, value_indices, center, extent);
}

build Octree::build_octree_recursive(std::shared_ptr<Octant>& root,
                                 const Eigen::MatirxXd& values,
                                 const std::vector<u_int>& value_indices,
                                 const Eigen::Vector3d& center,
                                 double extent){
    // 判断索引列表是否为空
    if(value_indices.size() == 0){
        return false;
    }
    // 判断当前节点是否为空
    if(root == nullptr){
        root.reset(center, extent, value_indices, false);
    }
    // 判断是否需要细分
    if(value_indices.size() < _leaf_size || extent < _min_extent){
        root->is_leaf = true;
    }else{
        // 计算每个点所属子节点
        std::map<u_char, std::vector<u_int>> children_value_indices;
        for(size_t i = 0; i < value_indices.size(); ++i){
            u_char morton_mode = 0;
            const Eigen::Vector3d& value_tmp = values.col(value_indices[i]);
            if(value_tmp(0) > center(0)) morton_mode |= 1;
            if(value_tmp(1) > center(1)) morton_mode |= 2;
            if(value_tmp(2) > center(2)) morton_mode |= 4;

            children_value_indices[morton].emplace_back(value_indices[i]);
        }
        // 递归构建子节点
        double factor[2] = {-0.5, 0.5};
        for(int i = 0; i < 8; ++i){
            Eigen::Vector3d child_center;
            child_center(0) = center[0] + factor[int((i & 1) > 0)] * extent;
            child_center(1) = center[1] + factor[int((i & 2) > 0)] * extent;
            child_center(2) = center[2] + factor[int((i & 4) > 0)] * extnet;
            double child_extent = 0.5 * extent;
            build_octree_recursive(root->children[i], values, 
                        children_value_indices[i], child_center, child_extent);
        }

    return true;
 }

2.3 代码测试

#include "Octree.h"

int main(){
    Eigen::MatrixXd data(3, 4);
    data << -1, 1, -1, 1,
            -1, -1, 1, 1,
            0, 0, 0, 0;

    Octree oct;
    oct.set_data(data);
    oct.build(1, 1);
    return 0;
}

参考链接:

  1. 机器学习算法(二十五):KD树详解及KD树最近邻算法_kdtree最近邻算法-CSDN博客
  2. PCL点云库学习笔记(2)——kd-tree和八叉树_kd树和八叉树-CSDN博客
  3. analyze_pointcloud/Nearest_Neighbors at master · QinZiwen/analyze_pointcloud
  • 22
    点赞
  • 20
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
以下是利用八叉树分割三维点云的Python代码示例: ```python import numpy as np from sklearn.neighbors import KDTree class Octree: def __init__(self, points, max_points_per_node=16, max_depth=8): self.points = points self.max_points_per_node = max_points_per_node self.max_depth = max_depth self.min_bounds = np.min(points, axis=0) self.max_bounds = np.max(points, axis=0) self.center = (self.min_bounds + self.max_bounds) / 2 self.children = None self.depth = 0 if len(points) > max_points_per_node and max_depth > 0: self.split() def split(self): self.children = [] for i in range(8): mask = np.array([i & 1, i & 2, i & 4]) bounds_min = np.where(mask, self.center, self.min_bounds) bounds_max = np.where(mask, self.max_bounds, self.center) indices = self.get_indices(bounds_min, bounds_max) if len(indices) > 0: child = Octree(self.points[indices], self.max_points_per_node, self.max_depth - 1) child.min_bounds = bounds_min child.max_bounds = bounds_max child.depth = self.depth + 1 self.children.append(child) def get_indices(self, min_bounds, max_bounds): indices = [] for i, point in enumerate(self.points): if (point >= min_bounds).all() and (point <= max_bounds).all(): indices.append(i) return indices def get_points_in_sphere(self, center, radius): indices = self.get_indices(center - radius, center + radius) if self.children is None: return indices else: result = [] for child in self.children: if (center - child.center).dot(center - child.center) < radius * radius: result += child.get_points_in_sphere(center, radius) return result def get_points_in_box(self, min_bounds, max_bounds): indices = self.get_indices(min_bounds, max_bounds) if self.children is None: return indices else: result = [] for child in self.children: if (min_bounds <= child.max_bounds).all() and (max_bounds >= child.min_bounds).all(): result += child.get_points_in_box(min_bounds, max_bounds) return result def get_points_in_cube(self, center, size): return self.get_points_in_box(center - size / 2, center + size / 2) def octree_from_points(points, max_points_per_node=16, max_depth=8): return Octree(points, max_points_per_node, max_depth) def octree_from_kd_tree(kd_tree, max_points_per_node=16, max_depth=8): return Octree(kd_tree.data, max_points_per_node, max_depth) def octree_from_file(filename, max_points_per_node=16, max_depth=8): points = np.loadtxt(filename) return Octree(points, max_points_per_node, max_depth) def kd_tree_from_points(points): return KDTree(points) def kd_tree_from_file(filename): points = np.loadtxt(filename) return KDTree(points) ``` 这个Octree类实现了八叉树的基本功能,包括分割点云、获取半径内的点、获取包含在指定盒子中的点等。可以使用octree_from_points、octree_from_kd_treeoctree_from_file函数来创建Octree对象。也可以使用kd_tree_from_points和kd_tree_from_file函数来创建KDTree对象。

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

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

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值