点云学习1

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

 

import open3d as o3d 

import os

import numpy as np

from pyntcloud import PyntCloud

 

def make_line_pcd(dv,meanData):

    '''

    定义三角形的点云

    :return:

    '''

    L = 400

    v0 = dv[:,0]

    v1 = dv[:,1]

    v2 = dv[:,2]

 

    triangle_points = np.array([[meanData[0],meanData[1],meanData[2]],  [v0[0]*L, v0[1]*L, v0[2]*L],

                                                                        [v1[0]*L/2, v1[1]*L/2, v1[2]*L/2],

                                                                        [v2[0]*L/3, v2[1]*L/3, v2[2]*L/3]], dtype=np.float32)

    lines = [[0, 1],[0,2],[0,3]]  # Right leg

    colors = [[0, 0, 1],[1, 0, 1],[0, 0, 0]]  # Default blue

    # 定义三角形的三个角点

    #point_pcd = open3d.geometry.PointCloud()  # 定义点云

    #point_pcd.points = o3d.utility.Vector3dVector(triangle_points)

 

    # 定义三角形三条连接线

    line_pcd = o3d.geometry.LineSet()

    line_pcd.lines = o3d.utility.Vector2iVector(lines)

    line_pcd.colors = o3d.utility.Vector3dVector(colors)

    line_pcd.points =o3d.utility.Vector3dVector(triangle_points)

 

    return line_pcd

 

# 功能：计算PCA的函数

# 输入：

#     data：点云，NX3的矩阵

#     correlation：区分np的cov和corrcoef，不输入时默认为False

#     sort: 特征值排序，排序是为了其他功能方便使用，不输入时默认为True

# 输出：

#     eigenvalues：特征值

#     eigenvectors：特征向量

 

def PCA(data, correlation=False, sort=True):

    # 作业1

    # 屏蔽开始

    meanData = np.mean(data,axis=0)

    #print("meanData : ",meanData)

    X = data - meanData

    N = X.shape[0]

    #print("N=",N)

    #covX = np.dot(X.T, X)/(N-1)

    covX = np.cov(X,rowvar=0)

    eigenvalues,eigenvectors = np.linalg.eig(covX)

    #print("eigenvalues = ",eigenvalues)

 

    # 屏蔽结束

 

    if sort:

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

        eigenvalues = eigenvalues[sort]

        eigenvectors = eigenvectors[:, sort]

 

    return eigenvalues, eigenvectors,meanData

 

def main():

    # 指定点云路径

    cat_index = 10 # 物体编号，范围是0-39，即对应数据集中40个物体

    root_dir = r'../../ModelNet40/ply_data' # 数据集路径

    cat = os.listdir(root_dir)

    filename = os.path.join(root_dir, cat[cat_index],'train', cat[cat_index]+'_0001.ply') # 默认使用第一个点云

 

    # 加载原始点云

    point_cloud_pynt = PyntCloud.from_file(filename)

    point_cloud_o3d = point_cloud_pynt.to_instance("open3d", mesh=False)

    #o3d.visualization.draw_geometries([point_cloud_o3d]) # 显示原始点云

 

    # 从点云中获取点，只对点进行处理

    points = point_cloud_pynt.points

    print('total points number is:', points.shape[0])

 

    # 用PCA分析点云主方向

    _, v, meanData = PCA(points)

    point_cloud_vector = v[:, 2] #点云主方向对应的向量

    print('the main orientation of this pointcloud is: ', point_cloud_vector)

    # TODO: 此处只显示了点云，还没有显示PCA

 

    #print("v0*v1 = ",np.dot(v[:,1],v[:,0]))

    line_pcd = make_line_pcd(v,meanData)

    #o3d.visualization.draw_geometries([point_cloud_o3d,line_pcd])

    

    # 循环计算每个点的法向量

    pcd_tree = o3d.geometry.KDTreeFlann(point_cloud_o3d)

    normals = np.zeros((points.shape[0],3))

    # 作业2

    # 屏蔽开始

    for i in range(points.shape[0]):

        [_, idx, _] = pcd_tree.search_radius_vector_3d(point_cloud_o3d.points[i], 20)

        #print("idx.shape = ", np.asarray(idx).shape)

        #pts_nn=[]

        

        ids = np.asarray(idx)

        pts_nn = np.asarray(points)[ids]

        _, v,_1 = PCA(np.asarray(pts_nn))

 

        normals[i,:]=v[:,2]

 

    # 由于最近邻搜索是第二章的内容，所以此处允许直接调用open3d中的函数

    #print("normals : ",normals)

    # 屏蔽结束

    normals = np.array(normals, dtype=np.float64)

    # TODO: 此处把法向量存放在了normals中

    point_cloud_o3d.normals = o3d.utility.Vector3dVector(normals)

    o3d.visualization.draw_geometries([point_cloud_o3d])

 

if __name__ == '__main__':

    main()

################################################################################

################################################################################

# 实现voxel滤波，并加载数据集中的文件进行验证

 

import open3d as o3d 

import os

import numpy as np

from pyntcloud import PyntCloud

 

def PCA(data, correlation=False, sort=True):

    # 作业1

    # 屏蔽开始

    meanData = np.mean(data,axis=0)

    #print("meanData : ",meanData)

    X = data - meanData

    N = X.shape[0]

    #print("N=",N)

    #covX = np.dot(X.T, X)/(N-1)

    covX = np.cov(X,rowvar=0)

    eigenvalues,eigenvectors = np.linalg.eig(covX)

    #print("eigenvalues = ",eigenvalues)

 

    # 屏蔽结束

 

    if sort:

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

        eigenvalues = eigenvalues[sort]

        eigenvectors = eigenvectors[:, sort]

 

    return eigenvalues, eigenvectors,meanData

#

def get_all_index(lst, item):

     return [i for i in range(len(lst)) if lst[i] == item]

 

# 功能：对点云进行voxel滤波

# 输入：

#     point_cloud：输入点云

#     leaf_size: voxel尺寸

def voxel_filter(point_cloud, leaf_size):

    filtered_points = []

    filtered_idx=[]

    # 作业3

    # 屏蔽开始

    minPC = np.min(point_cloud,axis=0)

    maxPC = np.max(point_cloud,axis=0)

    Dx = (maxPC[0] - minPC[0])/leaf_size

    Dy = (maxPC[1] - minPC[1])/leaf_size

    #Dz = (maxPC[2] - minPC[2])/leaf_size

    N = point_cloud.shape[0]

    #print("pc len = ",N)

    for i in range(N):

        idx = (np.asarray(point_cloud)[i][0] -  minPC[0] )//leaf_size + (np.asarray(point_cloud)[i][1] -  minPC[1] ) //leaf_size *Dx + (np.asarray(point_cloud)[i][2] -  minPC[2] )//leaf_size*Dx*Dy

        filtered_idx.append(idx)

    

    sort_idx = np.array(filtered_idx).argsort()[::-1]

 

    filtered_idx_ok = np.array(filtered_idx)[sort_idx]

    point_cloud_ok = np.asarray(point_cloud)[sort_idx]

 

    idx_set = set(filtered_idx_ok)

    for i in idx_set:

        all_idx = get_all_index(filtered_idx_ok,i)

        xm = np.mean(np.asarray(point_cloud_ok)[all_idx],axis=0)

        #print(xm)

        filtered_points.append(xm)

    # 屏蔽结束

 

    # 把点云格式改成array，并对外返回

    filtered_points = np.array(filtered_points, dtype=np.float64)

    return filtered_points

 

def main():

    # # 从ModelNet数据集文件夹中自动索引路径，加载点云

    cat_index = 10 # 物体编号，范围是0-39，即对应数据集中40个物体

    root_dir = r'../../ModelNet40/ply_data' # 数据集路径

    cat = os.listdir(root_dir)

    filename = os.path.join(root_dir, cat[cat_index],'train', cat[cat_index]+'_0001.ply') # 默认使用第一个点云

    point_cloud_pynt = PyntCloud.from_file(filename)

 

    # 加载自己的点云文件

    #file_name = "/Users/renqian/Downloads/program/cloud_data/11.ply"

    #point_cloud_pynt = PyntCloud.from_file(file_name)

 

    # 转成open3d能识别的格式

    point_cloud_o3d = point_cloud_pynt.to_instance("open3d", mesh=False)

    #o3d.visualization.draw_geometries([point_cloud_o3d]) # 显示原始点云

 

    # 调用voxel滤波函数，实现滤波

    filtered_cloud = voxel_filter(point_cloud_pynt.points, 5)

    point_cloud_o3d.points = o3d.utility.Vector3dVector(filtered_cloud)

    # 显示滤波后的点云

    #o3d.visualization.draw_geometries([point_cloud_o3d])

 

     # 循环计算每个点的法向量

    points = point_cloud_o3d.points

 

    pcd_tree = o3d.geometry.KDTreeFlann(point_cloud_o3d)

    N_pcd = np.asarray(points).shape[0]

    normals = np.zeros((N_pcd,3))

    # 作业2

    # 屏蔽开始

    for i in range(N_pcd):

        [_, idx, _] = pcd_tree.search_radius_vector_3d(point_cloud_o3d.points[i], 30)

        #print("idx.shape = ", np.asarray(idx).shape)

        #pts_nn=[]

        

        ids = np.asarray(idx)

        pts_nn = np.asarray(points)[ids]

        _, v,_1 = PCA(np.asarray(pts_nn))

 

        normals[i,:]=v[:,2]

 

    # 由于最近邻搜索是第二章的内容，所以此处允许直接调用open3d中的函数

    #print("normals : ",normals)

    # 屏蔽结束

    normals = np.array(normals, dtype=np.float64)

    # TODO: 此处把法向量存放在了normals中

    point_cloud_o3d.normals = o3d.utility.Vector3dVector(normals)

    o3d.visualization.draw_geometries([point_cloud_o3d])

 

if __name__ == '__main__':

    main()