open3d坡度滤波算法

import open3d as o3d
import numpy as np

# 读取点云数据或生成点云对象
point_cloud = o3d.io.read_point_cloud(r"C:\Users\peog\Desktop\volume\plane\a.pcd")
# o3d.io.write_point_cloud(r"C:\Users\peog\Desktop\volume\plane\c.pcd",point_cloud)
# 示例：生成随机点云数据
# points = np.random.rand(100, 3)
# point_cloud = o3d.geometry.PointCloud()
# point_cloud.points = o3d.utility.Vector3dVector(points)

# 计算法向量
point_cloud.estimate_normals( search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=50))
# o3d.geometry.estimate_normals
# 计算坡度
normals = np.asarray(point_cloud.normals)
slope = np.arctan2(np.sqrt(normals[:, 2] ** 2 + normals[:, 1] ** 2), normals[:, 0])

# 定义坡度阈值
slope_threshold = 0.2  # 示例阈值

# 进行斜坡滤波
slope_indices = np.where(slope < slope_threshold)



filtered_point_cloud = point_cloud.select_by_index(slope_indices[0])
filtered_point_cloud.paint_uniform_color([1,0,0])
# 可视化滤波后的点云
# o3d.visualization.draw_geometries([filtered_point_cloud])
o3d.io.write_point_cloud(r"C:\Users\peog\Desktop\volume\plane\d.pcd",filtered_point_cloud)

代码计算的是法线方向与地面水平方向的夹角，需要注意的是判断坐标系，即地面的高度方向是那个坐标轴，其中np.sqrt(normals[:, 2] ** 2 + normals[:, 1] ** 2)计算的是法线向量在YOZ平面上投影长度，np.arctan2(np.sqrt(normals[:, 2] ** 2 + normals[:, 1] ** 2), normals[:, 0])用于计算法线向量在x轴方向的分量和法线向量在YOZ构成的向量的弧度值。

 n=a+b+c,其中n为法线向量，a为x轴方向的分量，b为y轴方向的分量，c为z轴方向的分量。

如图，n=a+(b+c）