三维点云处理-KDTree和Octree

点云数据主要是表征目标表面的海量点集合，并不具备传统实体网格数据的集合拓扑信息。因此，如何建立离散点间的拓扑关系，实现基于邻域关系的快速查找也是点云数据处理中比较核心的问题。对于一维数据来说，典型的树形存储结构如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的方式如下：

给定初始化坐标轴，作为根节点分割超平面的法向量。如3-d tree，根节点通常选取为 X 轴；
随着树的深度的增加，循环的选取坐标轴，作为分割超平面的法向量，如3-d tree，根节点的左右子树选取 Y 轴，左右子树的下一级选择 Z 轴，再下一级选择 X轴，依次类推；
每次在进行当前节点的左右子树切分时，可以选择该维度上数据的中位数作为切分点。

如上图所示，每个非叶子节点都可以想象成一个分割超平面，用垂直于坐标轴的超平面将空间分成两个部分，不断对数据进行递归划分，直到所有数据都切分完成。对于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 的三维空间的几何对象进行划分，从而构成一个具有根节点的方向图。如下图所示：

2.1 Octree的构建

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

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

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);}// 递归构建Octreereturn 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;
}