https://github.com/mengxingshifen1218/learning-pointcloud/blob/master/%E6%B7%B1%E8%93%9D/CH1/PointCloudHomework1/pca_normal.py

KD-Tree原理详解

https://zhuanlan.zhihu.com/p/112246942

构建算法：

Input:  无序化的点云，维度k
Output：点云对应的kd-tree
Algorithm：
1、初始化分割轴：对每个维度的数据进行方差的计算，取最大方差的维度作为分割轴，标记为r；
2、确定节点：对当前数据按分割轴维度进行检索，找到中位数数据，并将其放入到当前节点上；
3、划分双支：
    划分左支：在当前分割轴维度，所有小于中位数的值划分到左支中；
    划分右支：在当前分割轴维度，所有大于等于中位数的值划分到右支中。
4、更新分割轴：r = (r + 1) % k;
5、确定子节点：
    确定左节点：在左支的数据中进行步骤2；
    确定右节点：在右支的数据中进行步骤2；

拿个例子说明为：

二维样例：{（2,3），（5,4），（9,6），（4,7），（8,1），（7,2）}

构建步骤：

1、初始化分割轴：

发现x轴的方差较大，所以，最开始的分割轴为x轴。

2、确定当前节点：

对{2，5，9，4，8，7}找中位数，发现{5,7}都可以，这里我们选择7，也就是(7,2);

3、划分双支数据：

在x轴维度上，比较和7的大小，进行划分：

左支：{(2,3)，(5,4)，(4,7)}

右支：{(9,6)，(8,1)}

4、更新分割轴：

一共就两个维度，所以，下一个维度是y轴。

5、确定子节点：

左节点：在左支中找到y轴的中位数(5,4)，左支数据更新为{(2,3)}，右支数据更新为{(4,7)}

右节点：在右支中找到y轴的中位数(9,6)，左支数据更新为{(8,1)}，右支数据为null。

6、更新分割轴：

下一个维度为x轴。

7、确定(5,4)的子节点：

左节点：由于只有一个数据，所以，左节点为(2,3)

右节点：由于只有一个数据，所以，右节点为(4,7)

8、确定(9,6)的子节点：

左节点：由于只有一个数据，所以，左节点为(8,1)

右节点：右节点为空。

最终，就可以构建整个的kd-tree了。

示意图如下所示 [1] ：

二维空间表示：

二维坐标系下的分割示意图

kd-tree表示：

2.3、最近邻检索

在构建了完整的kd-tree之后，我们想要使用他来进行高维空间的检索。所以，这里讲解一下比较常用的最近邻检索，其中范围检索也是同样的道理。

最近邻搜索，其实和之前我们曾经学习过的KNN很像。不过，在激光点云章，如果使用常规的KNN算法的话，时间复杂度会空前高涨。因此，为了减少时间消耗，在工程上，一般使用kd-tree进行最近邻检索。

由于kd-tree已经按照维度进行划分了，所以，我们在进行比较的时候，也可以通过维度进行比较，来快速定位到与其最接近的点。由于可能会进行多个最近邻点的检索，所以，算法也可能会发生变化。因此，我将从最简单的一个最近邻开始说起。

整体求解

 实现PCA分析和法向量计算，并加载数据集中的文件进行验证

import open3d as o3d 
# import os
import numpy as np
from scipy.spatial import KDTree

# from pyntcloud import PyntCloud

# 功能：计算PCA的函数
# 输入：
#     data：点云，NX3的矩阵
#     correlation：区分np的cov和corrcoef，不输入时默认为False
#     sort: 特征值排序，排序是为了其他功能方便使用，不输入时默认为True
# 输出：
#     eigenvalues：特征值
#     eigenvectors：特征向量
def PCA(data, correlation=False, sort=True):
    # normalize 归一化
    mean_data = np.mean(data, axis=0)
    normal_data = data - mean_data
    # 计算对称的协方差矩阵
    H = np.dot(normal_data.T, normal_data)
    # SVD奇异值分解，得到H矩阵的特征值和特征向量
    eigenvectors, eigenvalues, _ = np.linalg.svd(H)

    if sort:
        sort = eigenvalues.argsort()[::-1]
        eigenvalues = eigenvalues[sort]
        eigenvectors = eigenvectors[:, sort]

    return eigenvalues, eigenvectors

def main():

    # 从txt文件获取点，只对点进行处理
    filename = "D:\pointcloud_processing\modelnet40_normal_resampled\\airplane\\airplane_0001.txt"
    points = np.loadtxt(filename, delimiter=',')[:, 0:3] # 导入txt数据到np.array，这里只需导入前3列
    print('total points number is:', points.shape[0])

    # 用PCA分析点云主方向
    w, v = PCA(points) # PCA方法得到对应的特征值和特征向量
    point_cloud_vector = v[:, 0] #点云主方向对应的向量为最大特征值对应的特征向量
    print('the main orientation of this pointcloud is: ', point_cloud_vector)
    # 三个特征向量组成了三个坐标轴
    axis = o3d.geometry.TriangleMesh.create_coordinate_frame().rotate(v, center=(0, 0, 0))

    # 循环计算每个点的法向量
    leafsize = 32   # 切换为暴力搜索的最小数量
    KDTree_radius = 0.1 # 设置邻域半径
    tree = KDTree(points, leafsize=leafsize) # 构建KDTree
    radius_neighbor_idx = tree.query_ball_point(points, KDTree_radius) # 得到每个点的邻近索引
    normals = [] # 定义一个空list

    # -------------寻找法线---------------
    # 首先寻找邻域内的点
    for i in range(len(radius_neighbor_idx)):
        neighbor_idx = radius_neighbor_idx[i] # 得到第i个点的邻近点索引，邻近点包括自己
        neighbor_data = points[neighbor_idx] # 得到邻近点，在求邻近法线时没必要归一化，在PCA函数中归一化就行了
        eigenvalues, eigenvectors = PCA(neighbor_data) # 对邻近点做PCA，得到特征值和特征向量
        normals.append(eigenvectors[:, 2]) # 最小特征值对应的方向就是法线方向
    # ------------法线查找结束---------------
    normals = np.array(normals, dtype=np.float64) # 把法线放在了normals中
    # o3d.geometry.PointCloud，返回了PointCloud类型
    pc_view = o3d.geometry.PointCloud(points=o3d.utility.Vector3dVector(points))
    # 向PointCloud对象中添加法线
    pc_view.normals = o3d.utility.Vector3dVector(normals)
    # 可视化
    o3d.visualization.draw_geometries([pc_view, axis], point_show_normal=True)

if __name__ == '__main__':
    main()