open3D源码分析第五篇

2021SC@SDUSC

open3D点云应用

本篇结合之前对源码的分析，外加与小组成员讨论open3D中python方面关于点云的源码，我们结合最近所学的算法和计算机图形学方面的知识，用python完成了对open3D的两个应用。

计算点云的表面曲率

算法原理

若P点的特征值满足${\lambda }_0\le {\lambda }_1\le {\lambda }_2$，则P点的表面曲率为：$\delta =\delta =\frac{{\lambda }_0}{{\lambda }_0+{\lambda }_1+{\lambda }_2}$

$\delta$越小表明邻域越平坦，$\delta$越大则表明邻域的起伏变化越大。

代码实现

import open3d as o3d
import numpy as np

def pca_compute(data, sort=True):
    """
     SVD分解计算点云的特征值与特征向量
    :param data: 输入数据
    :param sort: 是否将特征值特征向量进行排序
    :return: 特征值与特征向量
    """
    average_data = np.mean(data, axis=0)  # 求均值
    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]  # 索引
    return eigenvalues

def caculate_surface_curvature(cloud, radius=0.003):
    """
    计算点云的表面曲率
    :param cloud: 输入点云
    :param radius: k近邻搜索的半径，默认值为：0.003m
    :return: 点云中每个点的表面曲率
    """
    points = np.asarray(cloud.points)
    kdtree = o3d.geometry.KDTreeFlann(cloud)
    num_points = len(cloud.points)
    curvature = []  # 储存表面曲率
    for i in range(num_points):
        k, idx, _ = kdtree.search_radius_vector_3d(cloud.points[i], radius)

        neighbors = points[idx, :]
        w = pca_compute(neighbors)  # w为特征值
        delt = np.divide(w[2], np.sum(w), out=np.zeros_like(w[2]), where=np.sum(w) != 0)
        curvature.append(delt)
    curvature = np.array(curvature, dtype=np.float64)
    return curvature

if __name__ == '__main__':

    pcd = o3d.io.read_point_cloud("data//1.pcd")
    surface_curvature = caculate_surface_curvature(pcd, radius=0.003)
    print(surface_curvature[:10])  # 输出前10个点的表面曲率

体素随机下采样

算法原理

1. 依据的点云数据坐标集合，求取 $X、Y、Z$三个坐标轴上的最大值$X_{max}、Y_{max}、Z_{max}$和最小值$X_{min}、Y_{min}、Z_{min}$。
2. 设置体素小栅格的边长r。
3. 根据 $X、Y、Z$三个坐标轴上的最大、最小值求得点云最小包围盒的边长$l_x、l_y、l_z$。
   
4. 计算体素网格的尺寸。
   
   式中，$\lfloor \rfloor$表示向下取整。
5. 计算点云中每一个点在体素小栅格内的索引h。
   

6.将h里的元素按照从小到大的顺序进行排序，每个体素小栅格内随机选取一个点代替小栅格内的所有点。

代码实现

import open3d as o3d
import numpy as np
import random

#体素随机下采样，第一个参数为输入的点云，第二个参数为体素分辨率
def voxel_random_filter(cloud, leaf_size):

    point_cloud = np.asarray(cloud.points)
    # 1、计算边界点
    x_min, y_min, z_min = np.amin(point_cloud, axis=0)
    x_max, y_max, z_max = np.amax(point_cloud, axis=0)
    # 2、计算体素格网的维度
    Dx = (x_max - x_min) // leaf_size + 1
    Dy = (y_max - y_min) // leaf_size + 1
    Dz = (z_max - z_min) // leaf_size + 1
    print("Dx x Dy x Dz is {} x {} x {}".format(Dx, Dy, Dz))
    # 3、计算每个点的格网索引
    h = list()  # h 为保存索引的列表
    for i in range(len(point_cloud)):
        hx = (point_cloud[i][0] - x_min) // leaf_size
        hy = (point_cloud[i][1] - y_min) // leaf_size
        hz = (point_cloud[i][2] - z_min) // leaf_size
        h.append(hx + hy * Dx + hz * Dx * Dy)
    h = np.array(h)
    #4、体素格网内随机筛选点
    h_indice = np.argsort(h)  #返回h里面的元素按从小到大排序的索引
    h_sorted = h[h_indice]
    random_idx = []
    begin = 0
    for i in range(len(h_sorted) - 1):
        if h_sorted[i] == h_sorted[i + 1]:
            continue
        else:
            point_idx = h_indice[begin: i + 1]
            random_idx.append(random.choice(point_idx))
        begin = i + 1
    filtered_points = (cloud.select_by_index(random_idx))
    return filtered_points


def visualizer_cloud(filtered):
    #显示滤波后的点云
    vis = o3d.visualization.Visualizer()
    vis.create_window(window_name='体素随机下采样结果可视化', width=800, height=600)
    #可视化参数设置
    opt = vis.get_render_option()
    opt.background_color = np.asarray([0, 0, 0])  #设置背景色
    opt.point_size = 1  #设置点的大小
    opt.show_coordinate_frame = False  #设置是否添加坐标系
    vis.add_geometry(filtered)  #加载点云到可视化窗口
    vis.run()  #激活显示窗口，这个函数将阻塞当前线程，直到窗口关闭。
    vis.destroy_window()  #销毁窗口，这个函数必须从主线程调用。


if __name__ == '__main__':
    #读取点云
    pcd = o3d.io.read_point_cloud("data//40m1.pcd")
    #调用函数实现下采样
    filtered_cloud = voxel_random_filter(pcd, 0.2)  #第二个参数为体素格网的边长
    print('滤波后的点数为：\n', filtered_cloud)
    visualizer_cloud(filtered_cloud)

小结

本篇结合之前对源码的分析，外加与小组成员讨论open3D中python方面关于点云的源码，我们结合最近所学的算法和计算机图形学方面的知识，用python完成了对open3D的两个应用。下一KDTree方面的源码分析。