【Lidar】Python实现点云CSF布料滤波算法提取地面点

这两天会持续更新一下Python处理点云数据的教程，大家可以点个关注。今天给大家分享一下点云的经典算法：CSF布料模拟算法。

1 CSF算法简介

CSF算法，全称为Cloth Simulation Filtering，是一种基于欧几里得空间中最小生成树思想的聚类算法，它可以很好地对点云数据进行分割和分类。这个算法的核心思想是把点云数据转化成一张图，然后通过逐步连接最近邻点构建出多棵生成树，再将所有生成树合并成一张图，并利用深度优先搜索、双向广搜等手段对其上点进行分割。这样就可以将点云数据快速分割成多个类别，并且获取到每个点所属的类别。

在CSF算法的基础上，可以使用其地面点滤波方法。具体方法是通过使用RANSAC算法估计地面模型的法向量，然后通过计算每个点到地面模型的距离来滤除非地面点，而最后剩下的点即为地面点。同时，需要在此基础上做一些修改来提高算法的效率：首先，将待处理的点云数据进行降采样，减少计算量；其次，使用KD树快速搜索每个点的最近邻点，加速点云数据的处理过程。

此外，CSF算法也可以结合其他技术进行优化，例如在数据预处理阶段使用数据降维技术，减少数据量；在生成树构建阶段使用多线程并行计算，提高计算效率；在分割阶段使用机器学习算法，提高分割精度等。

总之，CSF算法是一种基于图模型的聚类算法，适用于点云数据的分割和分类任务。通过与其他技术的结合，可以实现更高效、更精确的点云数据处理。

2 代码实现

2.1 项目地址

我这里的CSF算法是直接用了大佬的包，大家可以自行去下载，然后pip install安装即可。项目地址：GitHub - jianboqi/CSF

2.2 代码展示

我这里使用的是laspy库打开的点云文件，所以只支持.las格式的点云。你们也可以换成open3d库，这样就可以实现其他格式点云的CSF算法咯。

# -*- coding: utf-8 -*-
"""
@Time ： 2023/11/30 14:11
@Auth ： RS迷途小书童
@File ：Las Data CSF.py
@IDE ：PyCharm
@Purpose：点云数据CSF布料算法
"""
import laspy
import CSF
import numpy as np
import open3d as o3d


def csf_las(file=r"Z:\Personal\彭俊喜\Lidar_try/2.las", outfile=r"Z:\Personal\彭俊喜\Lidar_try/out1.las"):
    las = laspy.read(file)  # read a las file
    points = las.points
    xyz = np.vstack((las.x, las.y, las.z)).transpose()  # extract x, y, z and put into a list  # 点云的空间位置

    # ---------------------------CSF参数设置---------------------------
    csf = CSF.CSF()
    csf.params.bSloopSmooth = False  # 粒子设置为不可移动
    csf.params.cloth_resolution = 0.1  # 布料网格分辨率2
    csf.params.rigidness = 3  # 布料刚性参数2
    csf.params.time_step = 0.65  # 步长
    csf.params.class_threshold = 0.03  # 点云与布料模拟点的距离阈值0.5
    csf.params.interations = 500  # 最大迭代次数500
    # more details about parameter: http://ramm.bnu.edu.cn/projects/CSF/download/
    csf.setPointCloud(xyz)
    ground = CSF.VecInt()  # 地面点索引列表
    non_ground = CSF.VecInt()  # 非地面点索引列表
    csf.do_filtering(ground, non_ground)  # 执行滤波函数

    # ---------------------------laspy库保存----------------------------
    out_file = laspy.LasData(las.header)
    out_file.points = points[np.array(ground)]  # extract ground points, and save it to a las file.
    out_file.write(outfile)

    # ---------------------------open3d库保存---------------------------
    """
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(a)
    pcd1 = o3d.geometry.PointCloud()
    pcd1.points = o3d.utility.Vector3dVector(b)
    o3d.io.write_point_cloud(r'Z:/彭俊喜/1.pcd', pcd, write_ascii=False, compressed=False, print_progress=True)
    o3d.io.write_point_cloud(r'Z:/彭俊喜/2.pcd', pcd1, write_ascii=False, compressed=False, print_progress=True)
    """
    return xyz[ground], xyz[non_ground]
    # 返回地面点和非地面点的数组


def show_point(np1, np2):
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(np1)
    pcd1 = o3d.geometry.PointCloud()
    pcd1.points = o3d.utility.Vector3dVector(np2)
    # 数组转点云
    pcd.paint_uniform_color([0, 1, 0])  # 自定义颜色
    pcd1.paint_uniform_color([1, 0, 0])  # 自定义颜色
    o3d.visualization.draw_geometries([pcd], window_name='ground')
    o3d.visualization.draw_geometries([pcd1], window_name='tree')
    # o3d.visualization.draw_geometries([pcd], window_name='Point Cloud View', width=1920, height=1080, left=50, top=50,
    # point_show_normal=False, mesh_show_wireframe=False, mesh_show_back_face=False)


if __name__ == "__main__":
    gd, non_gd = csf_las()
    show_point(gd, non_gd)