三维点云处理之最近邻问题

前言：本系列文章是关于三维点云处理的常用算法，深入剖析pcl库中相关算法的实现原理，并以非调库的方式实现相应的demo。

1. 最近邻问题概述

（1）最邻近问题：对于点云中的点，怎么去找离它比较近的点

（2）获取邻域点的两种方法：KNN和RNN

• KNN：如图所示，红色点是要查找的点，蓝色点是数据库中的点，图中是找离红色点最近的3个点，显示出来就是图中的绿色点。
  

• Radius-NN
  ​ 以上述红色点为圆心，以所选值为半径画圆，圆内的点就是所要找的点
  
  （3）点云最近邻查找的难点

• 点云不规则

• 点云是三维的，比图像高一维，由此造成的数据量是指数上升的。当然，可以建一个三维网格，把点云转化为一个类似于三维图像的东西，但是这也会带来一些矛盾。因为如果网格很多，分辨率足够大，但处理网格需要的内存就很大；如果网格很少，内存够了，但是分辨率又太低。并且，网格中大部分区域都是空白的，所以网格从原理上就不是很高效。

• 点云数据量通常非常大。比如，一个64线的激光雷达，它每秒可产生2.2million个点，如果以20Hz的频率去处理，就意味着每50ms要处理110000个点，如果使用暴力搜索方法对这110000个点都找它最邻近的点，那么计算量为：$110000\times 110000\times 0.5=6\times 10^9$
  （4）最近邻查找：BST、Kd-tree、Octree的共同核心思想

• 空间分割
  ​ 将空间分割成多个部分，然后在每个小区域中去搜索

• 搜索停止条件
  ​ 若已知目标点到某一点的距离，那么对于超过这一距离的范围就不需要进行搜索，这个距离也被称为"worst distance"

2. 二叉树（Binary Search Tree, BST）

（1）二叉搜索树的特点（一维数据）

• 结点的左子树上的值都小于该根结点的值
• 结点的右子树上的值都大于该根节点的值
• 每一个左右子树都是一个BST
  
  （2）二叉树的构建——给定一串数字，如何构造出一个BST

class Node:
    def __init__(self, key, value=-1):
        self.left = None
        self.right = None
        self.key = key
        self.value = value  # 这里的value表示当前点的其他属性，比如颜色、编号等

Data generation ——  随机产生一串数字
    db_size = 10
    data = np.random.permutation(db_size).tolist()

Recursively insert each an element  ——  构造BST的具体实现
def insert(root, key, value=-1):
    if root is None:
        root = Node(key, value)
    else:
        if key < root.key:
            root.left = insert(root.left, key, value)
        elif key > root.key:
            root.right = insert(root.right, key, value)
        else:
            pass
    return root

Insert each element  ——  主函数调用
root = None
for i point in enumerate(data):
    root = insert(root, point, i)  # 这里的value（i）表示的是当前点在原始数组中的位置

（3）BST的复杂度

• 最坏情况下，二叉树的各结点顺次链接，排成一列，此时复杂度为$O(h)$，其中$h$为BST的高度，也是BST中结点个数
  
• 最好情况下，BST是处于平衡状态的，此时复杂度为$O(lo{g}_{2}n)$，$n$为BST中结点总数
  
  （4）二叉树查找——对于一个给定的点（数值），查找它是否在BST中

# 递归法
def search_recursive(root, key):
    if root is None or root.key == key:
        return root
    if key < root.key:    # 表明key在当前的左子树上
        return search_recursive(root.left, key)
    elif key > root.key: # 表明key在当前的右子树上
        return search_recursive(root.right, key)

# 迭代法  ——  通过栈来实现（不断迭代更新current_node）
def search_iterative(root, key):
    current_node = root
    while current_node is not None:
        if current_node.key == key:
            return current_node
        elif current_node.key < key:
            current_node = current_node.right
        elif current_node.key > key:
            current_node = current_node.left
    return current_node

（5）递归法与迭代法的特点

• 递归

好处：实现简单，容易理解，代码简短

坏处：由于递归需要不停地去压栈，所以每一次递归就是在内存中记录当前递归的位置，因此递归需要$O(n)$的内存空间，这里的$n$就是递归的次数

• 迭代

优点：它用一个量current_node来表示当前的位置，因此它所需的存储空间为$O(1)$；另外，由于GPU对于堆栈是比较困难的，往往只支持20多层的堆栈，很多时候是不够用的，可能会造成栈溢出（stack-overflow），而且在GPU上实现递归是非常慢的，迭代法可以避免这一问题

缺点：实现起来较为困难

（6）深度优先搜索 （Depth First Traversal）

# 前序遍历  ——  可用于复制一棵树
def preorder(root):
    if root is not None:
        print(root)
        preorder(root.left)
        preorder(root.right)
        
# 中序遍历  ——  可用于排序
def inorder(root):
    if root is not None:
        inorder(root.left)
        print(root)
        inorder(root.right)
        
# 后序遍历  ——  可用于删除一个结点
def postorder(root):
    if root is not None:
        postorder(root.left)
        postorder(root.right)
        print(root)

（7）KNN——寻找K个最近邻的点

​ 寻找当前点的K个最近邻点关键在于如何确定worst dist，具体步骤如下：

• 建立一个容器container来存储当前KNN的结果，并将容器中的结果进行排序：例如，当K = 6时，当前KNN结果为[1, 2, 3, 4, 4.5, inf]
• 容器中最后一个就是worst dist，对于新增结点，若新增结点与当前结点计算出来的dist小于当前worst dist，则将其添加到容器中，同时更新worst：例如，若新增结点计算出来的dist为6