3D点云处理1：PCA，法向量估计，体素滤波

目标

使用modelnet40数据集，对数据集中的数据进行原始点云展示、PCA投影、法向量估计、Centroid降采样与Random select 降采样
github链接

结果展示

从上至下分别为:airplane0001, plant0001, person0001

从左至右分别为: 原始点云、PCA投影、法向量估计、Centroid降采样、Random select降采样

PCA 与 法向量估计

算法原理

参考：

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

http://blog.codinglabs.org/articles/pca-tutorial.html

https://blog.csdn.net/u012162613/article/details/42177327

PCA

过程：

数据-> 去中心化

​ -> 协方差矩阵

​ -> 特征向量 -> 坐标轴方向

​ -> 特征值 -> 坐标轴方向的方差

降维：基的数量小于向量本身的维数

如何选择最优基？

N维向量 -> K维向量 ， 如何选择K个基使得信息损失最小

• 方差

  表示数值分散程度，变量的方差为每个元素与变量均值的差的平方和的均值
  $$Var(a)=\frac{1}{m}\sum _{i=1}^m(a_i-\mu {)}^2$$
  方便处理 -> 均值化为0
  $$Var(a)=\frac{1}{m}\sum _{i=1}^m{a}_i^2$$

• 协方差

  表示两个样本的相关性

  希望表示尽可能多的信息 -> 不存在线性相关性
  $$Cov(a,b)=\frac{1}{m-1}\sum _{i=1}^m(a_i-{\mu }_a)(b_i-{\mu }_b)$$
  均值为0：
  $$Cov(a,b)=\frac{1}{m}\sum _{i=1}^m{a}_i{b}_i$$

  优化目标：将一组 N 维向量降为 K 维，其目标是选择 K 个单位正交基，使得原始数据变换到这组基上后，各变量两两间协方差为 0，而变量方差则尽可能大（在正交的约束下，取最大的 K 个方差）。

• 协方差矩阵

  散度矩阵：
  $$C=X{X}^T$$
  对角线分别对应各个变量的方差，第 i 行 j 列和 j 行 i 列元素相同，表示 i 和 j 两个变量的协方差

  优化目标：将除对角元外的其余元素化为0，在对角线上将元素按大小从上到下排列（方差大）

• 矩阵对角化

  设原始数据矩阵 $X$ 对应的协方差矩阵为 $C$

  设矩阵 $P$ 为一组基按行组成的矩阵

  设矩阵 $Y$ 为 $X$ 对 $P$ 做基变换后的数据，$Y=PX$

  设矩阵 $Y$ 的协方差矩阵为 $D$

  $D$ 与 $C$ 的关系可推导如下：

优化目标：寻找矩阵 $P$ 使得 $PC{P}^T$ 为对角矩阵，且对角元按大小从上到下排列。矩阵 $P$ 的前 $k$ 行组成的矩阵与 $X$ 相乘即可得到降维结果 $Y$

由 $C_{n\times n}$ 为实对称矩阵可知，必可找到 $n$ 个单位正交特征向量，求之即可。

法向量估计

1. 选择一点P
2. 找到P的邻居节点
3. 对邻域节点PCA
4. 法向量->最不显著->最小的特征向量

核心代码

import os
import open3d as o3d
import numpy as np


def data_loader():
    folder_path = "./40_sample_data/"
    files = os.listdir(folder_path)
    points_data = []
    for file in files:
        file_path = folder_path + file
        points_data.append(np.loadtxt(file_path, delimiter=","))
    return points_data

def test_data_loader():
    file_path = "./40_sample_data/person_0001.txt"
    points_data = []
    points_data.append(np.loadtxt(file_path, delimiter=","))
    return points_data

def PCA(point_cloud_group, n):
    col_mean_val = np.mean(point_cloud_group, axis = 0)
    zero_mean_matrix = point_cloud_group - col_mean_val
    cov_matrix = np.cov(zero_mean_matrix, rowvar = 0)
    eig_val, eig_vector = np.linalg.eig(cov_matrix)
    origin_eig_val_index = np.argsort(eig_val)[::-1]
    origin_max_n_eig_val_index = origin_eig_val_index[:n]
    n_vector = eig_vector[:,origin_max_n_eig_val_index]
    low_point_data = zero_mean_matrix * np.mat(n_vector)
    return low_point_data, eig_vector[:,origin_eig_val_index[-1]]


if __name__ == "__main__":
    pcd_list = test_data_loader()
    for point in pcd_list:
        useful_point = point[:, :3]
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(useful_point)
        o3d.visualization.draw_geometries([pcd])
        # PCA
        PCA_point,_ = PCA(useful_point, 2)
        PCA_point = np.asarray(PCA_point)
        pcd_pca = o3d.geometry.PointCloud()
        tmp = np.zeros(np.shape(PCA_point)[0])
        PCA_3d_point = np.column_stack((PCA_point,tmp))
        pcd_pca.points = o3d.utility.Vector3dVector(PCA_3d_point)
        o3d.visualization.draw_geometries([pcd_pca])
        # estimate normals
        pcd_tree = o3d.geometry.KDTreeFlann(pcd) 
        normals = []
        points_array = np.asarray(pcd.points)
        for point in pcd.points:
            [_, idx, _] = pcd_tree.search_knn_vector_3d(point, knn = 30)
            # Tuple[int, open3d.utility.IntVector, open3d.utility.DoubleVector]
            knn_points = points_array[idx, : ]
            _, now_normal = PCA(knn_points, 3)
            normals.append(now_normal)
        normals = np.array(normals, dtype=np.float64)
        pcd.normals = o3d.utility.Vector3dVector(normals)
        o3d.visualization.draw_geometries([pcd], point_show_normal=True) # visualization

体素滤波

算法原理

参考：

https://blog.csdn.net/weixin_47309740/article/details/121908709

https://blog.csdn.net/HUASHUDEYANJING/article/details/125702480?spm=1001.2014.3001.5506

1. 获取点云坐标集合，分别获取 $X,Y,Z$ 三个坐标轴上的最大值 $x_{max},y_{max},z_{max}$ 和最小值 $x_{min},y_{min},z_{min}$

2. 设置体素小栅格边长 $r$

3. 依据求得的 $x_{max},y_{max},z_{max}$ 和 $x_{min},y_{min},z_{min}$ 可知点云集合最小包围盒的边长 $l_x,l_y,l_z$
   $$\begin{array}{c}\{\begin{array}{lr}{l}_x=x_{max}-x_{min}& \\ l_y=y_{max}-y_{min}& \\ l_z=z_{max}-z_{min}& \end{array}\end{array}$$

4. 计算体素网格尺寸
   $$\begin{array}{c}\{\begin{array}{lr}{D}_x=\lfloor l_x/r\rfloor & \\ D_y=\lfloor l_y/r\rfloor & \\ D_z=\lfloor l_y/r\rfloor & \end{array}\end{array}$$

5. 计算点云集合中的点在体素小栅格内的索引 $h$
   $$\begin{array}{c}\{\begin{array}{lr}{h}_x=\lfloor (x-x_{min})/r\rfloor & \\ h_y=\lfloor (y-y_{min})/r\rfloor & \\ h_z=\lfloor (z-z_{min})/r\rfloor & \\ h=h_x+h_y\ast D_x+h_z\ast D_x\ast D_y& \end{array}\end{array}$$

6. 将 $h$ 中的元素按从小到大的顺序进行排序（可选）

7. 根据Centroid或Random select进行采样

其中Centroid的实现方法为：

建立两个字典 dict_voxel 与 num_in_voxel

遍历点云，计算点云集合中的点在体素小栅格内的索引 $h$

若 $h$ 不是 dict_voxel 中的 $key$，则建立 $key$ 为 $h$ , $value$ 为 $point\_cloud[i]$ 的键值对加入字典 dict_voxel ，并建立 $key$ 为 $h$ , $value$ 为 $1$ 的键值对加入字典 num_in_voxel ，表示 $key$ $h$ 已经出现过一次

若 $h$ 是 dict_voxel 中的 $key$，则首先取出其在字典 dict_voxel 中的$value$ ，即此前的平均点云数据，之后取出其在字典 num_in_voxel 中的 $value$，即冲突次数，重新取平均值，修改 $h$ 在字典 dict_voxel 中对应的 $value$ 为新的平均值，修改 $h$ 在字典 num_in_voxel 中对应的 $value$ 为原先的 $出现次数+1$

遍历整个集合后，将字典中的所有 $value$ 保存，并进行格式转换即可

其中Random select的实现方法为：

建立一个字典 dict_voxel

遍历点云，计算点云集合中的点在体素小栅格内的索引 $h$

若 $h$ 不是 dict_voxel 中的 $key$，则建立 $key$ 为 $h$ , $value$ 为 $[point\_cloud[i]]$ 的键值对加入字典 dict_voxel ，其中$[point\_cloud[i]]$ 表示一个仅有 $point\_cloud[i]$ 的列表

若 $h$ 是 dict_voxel 中的 $key$，则首先取出其在字典 dict_voxel 中的$value$ ，即此前的点云列表，将现在的点云数据 $point\_cloud[i]$ 加入此列表中，更新 $h$ 在字典 dict_voxel 中对应的 $value$ 为新的列表

遍历整个集合后，根据 $key,value$ 遍历字典 dict_voxel ，其中每一个 $value$ 应为一个至少包含一个元素的列表，使用 random.choice()方法随机选择列表中的一个元素（一个点云数据），将其加入最终的结果列表中

遍历字典结束后，进行格式转换即可

核心代码

import os
import open3d as o3d
import numpy as np
import random

def data_loader():
    folder_path = "./40_sample_data/"
    files = os.listdir(folder_path)
    points_data = []
    for file in files:
        file_path = folder_path + file
        points_data.append(np.loadtxt(file_path, delimiter=","))
    return points_data

def test_data_loader():
    file_path = "./40_sample_data/person_0001.txt"
    points_data = []
    points_data.append(np.loadtxt(file_path, delimiter=","))
    return points_data

def Voxel_Grid_Downsampling(point_cloud, leaf_size=0.05, centroid_or_random='centroid'):
    '''
    input :
        point_cloud:点云数据(n,3) leaf_size:体素栅格边长 centroid_or_random:方法选择
    method:
        1. get x_min,y_min,z_min; x_max,y_max,z_max
        2. r = leaf_size
        3. cal l_x,l_y,l_z即点云最小包围盒边长
        4. 计算体素网格大小D_x,D_y,D_z
        5. 计算索引h
        6. 排序(可选)
    output:
        point_filter_res:滤波后的点云数据
    '''
    x_min,y_min,z_min = np.amin(point_cloud, axis=0)
    x_max,y_max,z_max = np.amax(point_cloud, axis=0)
    l_x = x_max - x_min
    l_y = y_max - y_min
    l_z = z_max - z_min
    D_x = l_x // leaf_size + 1
    D_y = l_y // leaf_size + 1
    D_z = l_z // leaf_size + 1

    res_point_list = []
    point_cloud_size = len(point_cloud)
    if centroid_or_random == 'centroid':
        dict_voxel = { }
        num_in_voxel = {}
        for i in range(point_cloud_size):
            h_x = (point_cloud[i,0] - x_min) // leaf_size + 1
            h_y = (point_cloud[i,1] - y_min) // leaf_size + 1
            h_z = (point_cloud[i,2] - z_min) // leaf_size + 1
            h = h_x + h_y * D_x + h_z * D_x * D_y
            if h not in dict_voxel:
                dict_voxel[h] = point_cloud[i]
                num_in_voxel[h] = 1
            else:
                avg_pos_pre = dict_voxel.get(h)
                num_pre = num_in_voxel[h]
                avg_pos_now = (avg_pos_pre * num_pre + point_cloud[i]) / (num_pre + 1)
                dict_voxel[h] = avg_pos_now
                num_in_voxel[h] = num_pre + 1
        for key,val in dict_voxel.items():
            res_point_list.append(val)
        point_filter_res = np.asarray(res_point_list, dtype=np.float64)
        return point_filter_res
    elif centroid_or_random == 'random':
        dict_voxel = { }
        for i in range(point_cloud_size):
            h_x = (point_cloud[i,0] - x_min) // leaf_size + 1
            h_y = (point_cloud[i,1] - y_min) // leaf_size + 1
            h_z = (point_cloud[i,2] - z_min) // leaf_size + 1
            h = h_x + h_y * D_x + h_z * D_x * D_y
            if h not in dict_voxel:
                dict_voxel[h] = [point_cloud[i]]
            else:
                point_list = dict_voxel.get(h)
                point_list.append(point_cloud[i])
                dict_voxel[h] = point_list
        for key, val in dict_voxel.items():
            select_point = random.choice(val)
            res_point_list.append(select_point)
        point_filter_res = np.asarray(res_point_list, dtype=np.float64)
        return point_filter_res


if __name__ == "__main__":
    pcd_list = test_data_loader()
    for pic_point in pcd_list:
        useful_point = pic_point[:, :3]
        point_filter_res = Voxel_Grid_Downsampling(useful_point, 0.05, 'random')
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(point_filter_res)
        o3d.visualization.draw_geometries([pcd])