4. 点云半径滤波去噪

任务目标

给定包含多个平面与非平面区域的点云数据，通过估计每个点的法向量，使用区域生长算法将具有相似法向量且空间邻近的点聚类为同一平面区域，并输出每个分割平面的模型参数（平面方程 ax+by+cz+d=0）。

环境依赖

需要 Open3D（≥0.15）与 NumPy。若需可视化，还需 matplotlib。安装命令：

pip install open3d numpy matplotlib

核心实现

代码完成以下闭环：生成含两个平面与一个球面的合成点云 → 估计法向量 → 区域生长分割 → 输出平面模型参数。分割核心逻辑基于法向量夹角与欧氏距离阈值，不依赖Open3D内置分割函数，便于理解算法原理。

import numpy as np
import open3d as o3d

def generate_scene():
    """生成合成点云：两个平面 + 一个球面"""
    # 平面1: z = 0.5
    pts1 = np.random.uniform([-2, -2, 0.5], [2, 2, 0.5], (3000, 3))
    # 平面2: x + y + z = 2
    u = np.random.uniform(-2, 2, (3000, 2))
    pts2 = np.column_stack([u[:, 0], u[:, 1], 2 - u[:, 0] - u[:, 1]])
    # 球面: 半径1.5，中心(0,0,0)
    phi = np.random.uniform(0, np.pi, 2000)
    theta = np.random.uniform(0, 2*np.pi, 2000)
    r = 1.5
    pts3 = np.column_stack([r*np.sin(phi)*np.cos(theta),
                            r*np.sin(phi)*np.sin(theta),
                            r*np.cos(phi)])
    return np.vstack([pts1, pts2, pts3]).astype(np.float64)

def region_growing_segmentation(pcd, normal_thresh_deg=15, dist_thresh=0.1):
    """基于法向量与距离的区域生长分割"""
    points = np.asarray(pcd.points)
    normals = np.asarray(pcd.normals)
    n = len(points)
    labels = np.full(n, -1, dtype=int)
    label_id = 0
    
    # 构建KDTree用于邻域查询
    pcd_tree = o3d.geometry.KDTreeFlann(pcd)
    
    for i in range(n):
        if labels[i] != -1:
            continue
        # BFS种子生长
        queue = [i]
        labels[i] = label_id
        while queue:
            idx = queue.pop(0)
            _, k_idx, _ = pcd_tree.search_knn_vector_3d(points[idx], 30)
            for j in k_idx:
                if labels[j] != -1:
                    continue
                # 法向量夹角检查
                cos_angle = np.abs(np.dot(normals[idx], normals[j]))
                angle_ok = cos_angle > np.cos(np.radians(normal_thresh_deg))
                # 距离检查
                dist_ok = np.linalg.norm(points[idx] - points[j]) < dist_thresh
                if angle_ok and dist_ok:
                    labels[j] = label_id
                    queue.append(j)
        label_id += 1
    return labels

def extract_plane_models(points, labels, min_points=500):
    """对每个分割区域用最小二乘拟合平面，返回 a,b,c,d"""
    models = []
    for lab in np.unique(labels):
        mask = labels == lab
        if np.sum(mask) < min_points:
            continue
        pts = points[mask]
        centroid = pts.mean(axis=0)
        _, _, vh = np.linalg.svd(pts - centroid)
        normal = vh[-1]  # 最小奇异值对应法向量
        d = -np.dot(normal, centroid)
        models.append((lab, normal[0], normal[1], normal[2], d, int(np.sum(mask))))
    return models

# ---- 主流程 ----
cloud = generate_scene()
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(cloud)

# 1. 法向量估计（半径搜索，适应点密度）
pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.3, max_nn=30))
pcd.orient_normals_consistent_tangent_plane(30)

# 2. 区域生长分割
labels = region_growing_segmentation(pcd)

# 3. 提取平面模型
models = extract_plane_models(cloud, labels)
print("检测到的平面区域数:", len(models))
for lab, a, b, c, d, cnt in models:
    print(f"区域{lab}: {a:.3f}x + {b:.3f}y + {c:.3f}z + {d:.3f} = 0, 点数={cnt}")

运行与验证

运行方式：将上述代码保存为 plane_seg.py，直接执行 python plane_seg.py。

预期输出：程序应打印2个平面模型（球面点因法向量连续变化且曲率大，会被分割成多个小区域，但点数均小于min_points阈值而被过滤）。两个平面的法向量应分别接近 (0,0,1) 和 (0.577, 0.577, 0.577)（即归一化的 (1,1,1)），d值分别约为 -0.5 和 -1.155。

验证标准：

• 输出恰好为2个平面区域，且点数均大于500；
• 第一个平面法向量 z 分量值 > 0.99，第二个平面法向量各分量值在 0.5~0.7 之间；
• 拟合残差（点到平面平均距离）< 0.02，可通过在代码中追加 np.mean(np.abs(pts @ normal + d)) 验证。

关键参数说明

• normal_thresh_deg=15：相邻点法向量夹角小于15°才归为同一区域，控制分割的平滑度；
• dist_thresh=0.1：空间距离阈值，防止非连续区域被错误连接；
• min_points=500：过滤过小区域，球面碎片通常小于此值。

上图展示了合成场景中两个平面（保留）与球面（因法向量连续变化被分割为小碎片后过滤）的分割逻辑。区域生长从种子点出发，仅当邻域点法向量夹角小于阈值且距离足够近时才归入同一区域，最终通过最小二乘拟合提取平面方程系数。