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

最新推荐文章于 2024-07-07 23:24:12 发布

Code&Coke

最新推荐文章于 2024-07-07 23:24:12 发布

阅读量827

点赞数 2

分类专栏： 3D点云处理文章标签：线性代数机器学习 3d

本文链接：https://blog.csdn.net/weixin_45568867/article/details/129668569

版权

3D点云处理专栏收录该内容

1 篇文章 0 订阅

订阅专栏

目标

使用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个基使得信息损失最小

方差

表示数值分散程度，变量的方差为每个元素与变量均值的差的平方和的均值
$\frac{1}{m}\sum_{i=1}^{m}(a_{i}-\mu)^{2}$
方便处理 -> 均值化为0
$\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=XX^{T}$
对角线分别对应各个变量的方差，第 i 行 j 列和 j 行 i 列元素相同，表示 i 和 j 两个变量的协方差

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

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

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

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

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

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

在这里插入图片描述

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

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

法向量估计

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

核心代码

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

获取点云坐标集合，分别获取 $X, Y, Z$ 三个坐标轴上的最大值 $x_{max},y_{max},z_{max}$ 和最小值 $x_{min},y_{min},z_{min}$
设置体素小栅格边长 $r$
依据求得的 $x_{max},y_{max},z_{max}$ 和 $x_{min},y_{min},z_{min}$ 可知点云集合最小包围盒的边长 $l_{x},l_{y},l_{z}$
$\begin{equation} \left\{ \begin{array}{lr} l_{x}=x_{max}-x_{min} & \\ l_{y}=y_{max}-y_{min} \\ l_{z}=z_{max}-z_{min} & \end{array} \right. \end{equation}$
计算体素网格尺寸
$\begin{equation} \left \{\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} \right. \end{equation}$
计算点云集合中的点在体素小栅格内的索引 $h$
$\begin{equation}\left\{\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}*D_{x}+h_{z}*D_{x}*D_{y} \end{array}\right.\end{equation}$
将 $h$ 中的元素按从小到大的顺序进行排序（可选）
根据Centroid或Random select进行采样

其中Centroid的实现方法为：

建立两个字典 dict_voxel 与 num_in_voxel

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

若 $h$ 不是 dict_voxel 中的 $k ey$ ，则建立 $k ey$ 为 $h$ , $v a l u e$ 为 $point\_ cloud[i]$ 的键值对加入字典 dict_voxel ，并建立 $k ey$ 为 $h$ , $v a l u e$ 为 $1$ 的键值对加入字典 num_in_voxel ，表示 $k ey$ $h$ 已经出现过一次

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

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

其中Random select的实现方法为：

建立一个字典 dict_voxel

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

若 $h$ 不是 dict_voxel 中的 $k ey$ ，则建立 $k ey$ 为 $h$ , $v a l u e$ 为 $point\_ cloud[i]]$ 的键值对加入字典 dict_voxel ，其中 $point\_ cloud[i]]$ 表示一个仅有 $point\_ cloud[i]$ 的列表

若 $h$ 是 dict_voxel 中的 $k ey$ ，则首先取出其在字典 dict_voxel 中的 $v a l u e$ ，即此前的点云列表，将现在的点云数据 $point\_ cloud[i]$ 加入此列表中，更新 $h$ 在字典 dict_voxel 中对应的 $v a l u e$ 为新的列表

遍历整个集合后，根据 $k ey, v a l u e$ 遍历字典 dict_voxel ，其中每一个 $v a l u e$ 应为一个至少包含一个元素的列表，使用 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])