5.点云法向量的计算(代码)

本文看了博主三维点云学习（1）上-PCA主成分分析 法向量估计_主成分分析法表面法向量估计-CSDN博客的文章，内容基本上参考了该博文，在这里写只是为了记笔记，没有要抄袭的意思，如果要更详细的了解，可以去看该博文。

首先，读取点云数据并且处理点云数据，我下载的一个点云数据文件不是pcd文件，我首先读取了点云数据，并将其处理为array数据，该数据的shape为(641, 3)，表示有641个xyz的数据，后面的代码和前面博文的相同（该代码用到了pca主成分分析原理，也很好理解，在前面博文有解释，要单独理解主成分分析原理，可以看我的另一篇博文12.1 主成分分析原理（PCA)_主成分分析零均值化-CSDN博客，该博文详细解释了主成分分析原理），代码如下：

import numpy as np
import open3d as o3d
 
import open3d as o3d
import os
import numpy as np
import matplotlib.pyplot as plt
from pandas import DataFrame
from pyntcloud import PyntCloud
 
# 聚类算法
def cluster(points, radius=100, nums=250):
    """
    points: pointcloud
    radius: max cluster range
    """
    print("................", len(points))
    items = []
    while len(points) > 1:
        item = np.array([points[0]])
        base = points[0]
        points = np.delete(points, 0, 0)
        distance = (points[:, 0] - base[0]) ** 2 + (points[:, 1] - base[1]) ** 2  + (points[:, 2] - base[2]) ** 2# 获得距离
        infected_points = np.where(distance <= radius ** 2)  # 与base距离小于radius**2的点的坐标
        item = np.append(item, points[infected_points], axis=0)
        border_points = points[infected_points]
        points = np.delete(points, infected_points, 0)
        while len(border_points) > 0:
            border_base = border_points[0]
            border_points = np.delete(border_points, 0, 0)
            border_distance = (points[:, 0] - border_base[0]) ** 2 + (points[:, 1] - border_base[1]) ** 2
            border_infected_points = np.where(border_distance <= radius ** 2)
            # print("/",border_infected_points)
            item = np.append(item, points[border_infected_points], axis=0)
            if len(border_infected_points) > 0:
                for k in border_infected_points:
                    if points[k] not in border_points:
                        border_points = np.append(border_points, points[k], axis=0)
            points = np.delete(points, border_infected_points, 0)
        if len(item) >= nums:
            items.append(item)
    return items
 
# matplotlib显示点云函数
def Point_Cloud_Show(points):
    fig = plt.figure(dpi=150)
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(points[:, 0], points[:, 1], points[:, 2], cmap='spectral', s=2, linewidths=0, alpha=1, marker=".")
    plt.title('Point Cloud')
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_zlabel('z')
    plt.show()
 
 
# 二维点云显示函数
def Point_Show(pca_point_cloud):
    x = []
    y = []
    pca_point_cloud = np.asarray(pca_point_cloud)
    for i in range(640):
        x.append(pca_point_cloud[i][0])
        y.append(pca_point_cloud[i][1])
    plt.scatter(x, y)
    plt.show()
 
 
 
# 功能：计算PCA的函数
# 输入：
#     data：点云，NX3的矩阵
#     correlation：区分np的cov和corrcoef，不输入时默认为False
#     sort: 特征值排序，排序是为了其他功能方便使用，不输入时默认为True
# 输出：
#     eigenvalues：特征值
#     eigenvectors：特征向量
def PCA(data, correlation=False, sort=True):
    # 作业1
    # 屏蔽开始
    average_data = np.mean(data, axis=0)  # 求 NX3 向量的均值
    decentration_matrix = data - average_data  # 去中心化
    H = np.dot(decentration_matrix.T, decentration_matrix)  # 求解协方差矩阵 H
    eigenvectors, eigenvalues, eigenvectors_T = np.linalg.svd(H)  # SVD求解特征值、特征向量
    # 屏蔽结束
 
    if sort:
        sort = eigenvalues.argsort()[::-1]  # 降序排列
        eigenvalues = eigenvalues[sort]  # 索引
        eigenvectors = eigenvectors[:, sort]
 
    return eigenvalues, eigenvectors
 
 
if __name__ == '__main__':
    # 1.读取数据
    datas_path = "datas/pine/111.txt"
 
    ss = []
    with open(datas_path, "r") as f:  # 打开文件
        data = f.readlines()  # 读取文件
    for item in data:
        t = np.asarray(item[:-1].split(" ")[2:])
        ss.append([float(t[0]) ,float(t[1]) ,float(t[2])])
    datas = np.asarray(ss)
    print(datas.shape)
 
    # items = cluster(datas, radius=30, nums=100)
    # da = np.asarray(items)
    # da = da.reshape((da.shape[1],da.shape[2]))
    # print(da)
    # pcd = o3d.geometry.PointCloud()
    # pcd.points = o3d.utility.Vector3dVector(da)
    # o3d.visualization.draw_geometries([pcd])
    da = datas
    point_cloud_raw = DataFrame(da[:, 0:3])  # 选取每一列 的 第0个元素到第二个元素   [0,3)
    point_cloud_raw.columns = ['x', 'y', 'z']  # 给选取到的数据 附上标题
    point_cloud_pynt = PyntCloud(point_cloud_raw)  # 将points的数据 存到结构体中
 
    point_cloud_o3d = point_cloud_pynt.to_instance("open3d", mesh=False)  # 实例化
 
    # o3d.visualization.draw_geometries([point_cloud_o3d])  # 显示原始点云
 
    # 用PCA分析点云主方向
    w, v = PCA(point_cloud_raw)  # w为特征值 v为主方向
    point_cloud_vector1 = v[:, 0]  # 点云主方向对应的向量，第一主成分
    point_cloud_vector2 = v[:, 1]  # 点云主方向对应的向量，第二主成分
    point_cloud_vector = v[:, 0:2]  # 点云主方向与次方向
    print('the main orientation of this pointcloud is: ', point_cloud_vector1)
    print('the main orientation of this pointcloud is: ', point_cloud_vector2)
 
    # 在原点云中画图
    point = [[0, 0, 0], point_cloud_vector1, point_cloud_vector2]  # 画点：原点、第一主成分、第二主成分
    lines = [[0, 1], [0, 2]]  # 画出三点之间两两连线
    colors = [[1, 0, 0], [0, 0, 0]]
    # 构造open3d中的LineSet对象，用于主成分显示
    line_set = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(point), lines=o3d.utility.Vector2iVector(lines))
    line_set.colors = o3d.utility.Vector3dVector(colors)
    o3d.visualization.draw_geometries([point_cloud_o3d, line_set])  # 显示原始点云和PCA后的连线
 
    # 将原数据进行降维度处理
    point_cloud_encode = (np.dot(point_cloud_vector.T, point_cloud_raw.T)).T  # 主成分的转置 dot 原数据
    Point_Show(point_cloud_encode)
    # 使用主方向进行升维
    point_cloud_decode = (np.dot(point_cloud_vector, point_cloud_encode.T)).T
    Point_Cloud_Show(point_cloud_decode)
 
    # 循环计算每个点的法向量
    pcd_tree = o3d.geometry.KDTreeFlann(point_cloud_o3d)  # 将原始点云数据输入到KD,进行近邻取点
    normals = []  # 储存曲面的法向量
    # 作业2
    # 屏蔽开始
    print(point_cloud_raw.shape[0])  # 打印当前点数 20000个点
    for i in range(point_cloud_raw.shape[0]):
        # search_knn_vector_3d函数 ， 输入值[每一点，x]      返回值 [int, open3d.utility.IntVector, open3d.utility.DoubleVector]
        [_, idx, _] = pcd_tree.search_knn_vector_3d(point_cloud_o3d.points[i], 10)  # 取10个临近点进行曲线拟合
        # asarray和array 一样 但是array会copy出一个副本，asarray不会，节省内存
        k_nearest_point = np.asarray(point_cloud_o3d.points)[idx, :]  # 找出每一点的10个临近点，类似于拟合成曲面，然后进行PCA找到特征向量最小的值，作为法向量
        w, v = PCA(k_nearest_point)
        normals.append(v[:, 2])
 
    # # 屏蔽结束
    normals = np.array(normals, dtype=np.float64)
    # print(normals)
    # # TODO: 此处把法向量存放在了normals中
    point_cloud_o3d.normals = o3d.utility.Vector3dVector(normals)
    o3d.visualization.draw_geometries([point_cloud_o3d])

        注意：上面代码比前面博文的代码多加了个聚类算法，在该代码中屏蔽了没用，如果想先用聚类处理点云，可以直接用上面的聚类函数处理点云。在这里可以结合理论和代码详细了解pca主成分分析的原理及应用，该主成分分析也可以通用于其他项目。