import open3d as o3d import numpy as np from scipy.spatial import KDTree import copy import argparse import os # ========== 1. Chamfer Distance 计算 ========== def chamfer_distance(pcd1, pcd2, sqrt=True): """ 计算两个点云之间的 CD 距离（Chamfer Distance） Args: pcd1: Open3D 点云对象1 pcd2: Open3D 点云对象2 sqrt: 是否对最终结果开平方根 Returns: cd: Chamfer Distance 值 cd1: 点云1 到 点云2 的平均距离 cd2: 点云2 到 点云1 的平均距离 """ points1 = np.asarray(pcd1.points) points2 = np.asarray(pcd2.points) if len(points1) == 0 or len(points2) == 0: return float('inf'), 0, 0 tree1 = KDTree(points1) tree2 = KDTree(points2) dist1, _ = tree2.query(points1, k=1) cd1 = np.mean(dist1 ** 2) dist2, _ = tree1.query(points2, k=1) cd2 = np.mean(dist2 ** 2) cd = cd1 + cd2 if sqrt: return np.sqrt(cd), cd1, cd2 return cd, cd1, cd2 # ========== 添加高斯噪声 ========== def add_gaussian_noise(pcd, noise_std=4.0): """ 给点云添加高斯噪声 Args: pcd: Open3D 点云对象 noise_std: 噪声标准差 Returns: noisy_pcd: 添加噪声后的点云 """ noisy_pcd = copy.deepcopy(pcd) points = np.asarray(noisy_pcd.points) noise = np.random.normal(0, noise_std, points.shape) noisy_points = points + noise noisy_pcd.points = o3d.utility.Vector3dVector(noisy_points) return noisy_pcd # ========== 2. 统计离群点移除 (SOR) ========== def statistical_outlier_removal(pcd, nb_neighbors=20, std_ratio=2.0): """ 统计离群点移除 """ cl, ind = pcd.remove_statistical_outlier(nb_neighbors=nb_neighbors, std_ratio=std_ratio) inlier_cloud = pcd.select_by_index(ind) outlier_cloud = pcd.select_by_index(ind, invert=True) metrics = { 'original_points': len(pcd.points), 'filtered_points': len(inlier_cloud.points), 'removed_points': len(outlier_cloud.points), 'keep_ratio': len(inlier_cloud.points) / len(pcd.points) * 100 if len(pcd.points) > 0 else 0, 'method': 'SOR', 'params': {'nb_neighbors': nb_neighbors, 'std_ratio': std_ratio} } return inlier_cloud, outlier_cloud, metrics # ========== 3. 半径离群点移除 (ROR) ========== def radius_outlier_removal(pcd, nb_points=5, radius=20): """ 半径离群点移除 """ cl, ind = pcd.remove_radius_outlier(nb_points=nb_points, radius=radius) inlier_cloud = pcd.select_by_index(ind) outlier_cloud = pcd.select_by_index(ind, invert=True) metrics = { 'original_points': len(pcd.points), 'filtered_points': len(inlier_cloud.points), 'removed_points': len(outlier_cloud.points), 'keep_ratio': len(inlier_cloud.points) / len(pcd.points) * 100 if len(pcd.points) > 0 else 0, 'method': 'ROR', 'params': {'nb_points': nb_points, 'radius': radius} } return inlier_cloud, outlier_cloud, metrics # ========== 4. 直通滤波 (PF) ========== def pass_through_filter(pcd, axis='z', min_val=-6000.0, max_val=6000.0): """ 直通滤波 """ points = np.asarray(pcd.points) axis_map = {'x': 0, 'y': 1, 'z': 2} idx = axis_map[axis] mask = (points[:, idx] >= min_val) & (points[:, idx] <= max_val) filtered_points = points[mask] filtered_pcd = o3d.geometry.PointCloud() filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points) metrics = { 'original_points': len(pcd.points), 'filtered_points': len(filtered_points), 'removed_points': len(pcd.points) - len(filtered_points), 'keep_ratio': len(filtered_points) / len(pcd.points) * 100 if len(pcd.points) > 0 else 0, 'method': 'PF', 'params': {'axis': axis, 'min_val': min_val, 'max_val': max_val} } return filtered_pcd, metrics def compute_keep_ratio_and_density(pcd, filtered_pcd, sample_size=1000): """计算直通滤波的保留率和密度变化""" n_orig = len(pcd.points) n_filt = len(filtered_pcd.points) if n_orig == 0 or n_filt == 0: return 0, 0 keep_ratio = n_filt / n_orig * 100 points_orig = np.asarray(pcd.points) points_filt = np.asarray(filtered_pcd.points) if len(points_orig) > sample_size: idx_orig = np.random.choice(len(points_orig), sample_size, replace=False) points_orig_sample = points_orig[idx_orig] else: points_orig_sample = points_orig if len(points_filt) > sample_size: idx_filt = np.random.choice(len(points_filt), sample_size, replace=False) points_filt_sample = points_filt[idx_filt] else: points_filt_sample = points_filt if len(points_orig_sample) > 5: tree_orig = KDTree(points_orig_sample) dist_orig, _ = tree_orig.query(points_orig_sample, k=2) avg_dist_orig = np.mean(dist_orig[:, 1]) else: avg_dist_orig = 0 if len(points_filt_sample) > 5: tree_filt = KDTree(points_filt_sample) dist_filt, _ = tree_filt.query(points_filt_sample, k=2) avg_dist_filt = np.mean(dist_filt[:, 1]) else: avg_dist_filt = 0 if avg_dist_orig > 0: density_change = (avg_dist_filt - avg_dist_orig) / avg_dist_orig * 100 else: density_change = 0 return keep_ratio, density_change # ========== 5. 体素滤波 (VGF) ========== def voxel_grid_filter(pcd, leaf_size=0.05): """ 体素滤波 """ filtered_pcd = pcd.voxel_down_sample(leaf_size) n_orig = len(pcd.points) n_filt = len(filtered_pcd.points) metrics = { 'original_points': n_orig, 'filtered_points': n_filt, 'removed_points': n_orig - n_filt, 'keep_ratio': n_filt / n_orig * 100 if n_orig > 0 else 0, 'downsample_rate': (n_orig - n_filt) / n_orig * 100 if n_orig > 0 else 0, 'compression_ratio': n_orig / n_filt if n_filt > 0 else 0, 'method': 'VGF', 'params': {'leaf_size': leaf_size} } if n_filt > 5: points_sample = np.asarray(filtered_pcd.points) sample_size = min(1000, n_filt) if n_filt > sample_size: idx = np.random.choice(n_filt, sample_size, replace=False) points_sample = points_sample[idx] tree = KDTree(points_sample) dist, _ = tree.query(points_sample, k=2) metrics['avg_point_distance'] = np.mean(dist[:, 1]) return filtered_pcd, metrics # ========== 6. 双边滤波 (BF) ========== def bilateral_filter(pcd, sigma_s=0.01, sigma_r=0.1): """ 双边滤波 """ if len(pcd.points) == 0: return pcd, {'original_points': 0, 'filtered_points': 0, 'removed_points': 0, 'keep_ratio': 0, 'method': 'BF', 'params': {}} # 确保有法线 if not pcd.has_normals(): pcd.estimate_normals() points = np.asarray(pcd.points) normals = np.asarray(pcd.normals) tree = KDTree(points) filtered_points = np.zeros_like(points) for i, p in enumerate(points): indices = tree.query_ball_point(p, r=2 * sigma_s) if len(indices) <= 1: filtered_points[i] = p continue neigh_points = points[indices] neigh_normals = normals[indices] spatial_dist = np.linalg.norm(neigh_points - p, axis=1) w_s = np.exp(-spatial_dist ** 2 / (2 * sigma_s ** 2)) normal_diff = np.linalg.norm(neigh_normals - normals[i], axis=1) w_r = np.exp(-normal_diff ** 2 / (2 * sigma_r ** 2)) w = w_s * w_r if w.sum() > 0: w = w / w.sum() filtered_points[i] = np.sum(neigh_points.T * w, axis=1) filtered_pcd = o3d.geometry.PointCloud() filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points) filtered_pcd.normals = o3d.utility.Vector3dVector(normals) metrics = { 'original_points': len(pcd.points), 'filtered_points': len(filtered_points), 'removed_points': 0, 'keep_ratio': 100, 'method': 'BF', 'params': {'sigma_s': sigma_s, 'sigma_r': sigma_r} } return filtered_pcd, metrics # ========== 主函数 ========== def main(): parser = argparse.ArgumentParser(description='点云去噪算法对比 (SOR, ROR, PF, VGF, BF)') parser.add_argument('--ply_path', '-p', type=str, default="./High_Speed_Data/PointCloud/11.ply", help='输入点云文件路径 (.ply)') parser.add_argument('--noise_std', '-n', type=float, default=4.0, help='高斯噪声标准差 (默认: 4.0)') parser.add_argument('--voxel_size', '-v', type=float, default=0.05, help='体素滤波大小 (默认: 0.05)') parser.add_argument('--sor_neighbors', type=int, default=20, help='SOR 邻域点数 (默认: 20)') parser.add_argument('--sor_std_ratio', type=float, default=2.0, help='SOR 标准差倍数 (默认: 2.0)') parser.add_argument('--ror_points', type=int, default=5, help='ROR 半径内最小点数 (默认: 5)') parser.add_argument('--ror_radius', type=float, default=20, help='ROR 搜索半径 (默认: 20)') parser.add_argument('--bf_sigma_s', type=float, default=0.01, help='双边滤波空间标准差 (默认: 0.01)') parser.add_argument('--bf_sigma_r', type=float, default=0.1, help='双边滤波特征标准差 (默认: 0.1)') parser.add_argument('--no_visualize', action='store_true', help='不显示可视化窗口') args = parser.parse_args() # 检查文件是否存在 if not os.path.exists(args.ply_path): print(f"错误: 点云文件不存在 - {args.ply_path}") return # 1. 读取原始点云 print(f"读取点云: {args.ply_path}") pcd_clean = o3d.io.read_point_cloud(args.ply_path) if len(pcd_clean.points) == 0: print("读取失败，请检查文件路径") return print(f"原始无噪点云点数: {len(pcd_clean.points)}") # ========== 添加高斯噪声 ========== print("\n" + "=" * 50) print(f"添加高斯噪声 (std={args.noise_std})") print("=" * 50) pcd_noisy = add_gaussian_noise(pcd_clean, args.noise_std) print(f"加噪后点云点数: {len(pcd_noisy.points)}") # 计算噪声水平 points_clean = np.asarray(pcd_clean.points) points_noisy = np.asarray(pcd_noisy.points) noise_level = np.mean(np.linalg.norm(points_noisy - points_clean, axis=1)) print(f"平均噪声偏移量: {noise_level:.4f}") # 加噪点云着色 pcd_noisy.paint_uniform_color([1.0, 0.7, 0.5]) # ========== 运行各去噪算法 ========== results = {} # 1. SOR print("\n" + "=" * 50) print("统计离群点移除 (SOR)") print("=" * 50) sor_filtered, sor_outlier, sor_metrics = statistical_outlier_removal( pcd_noisy, nb_neighbors=args.sor_neighbors, std_ratio=args.sor_std_ratio) results['SOR'] = sor_metrics print(f"保留率: {sor_metrics['keep_ratio']:.2f}%") print(f"移除: {sor_metrics['removed_points']} 点") # 2. ROR print("\n" + "=" * 50) print("半径离群点移除 (ROR)") print("=" * 50) ror_filtered, ror_outlier, ror_metrics = radius_outlier_removal( pcd_noisy, nb_points=args.ror_points, radius=args.ror_radius) results['ROR'] = ror_metrics print(f"保留率: {ror_metrics['keep_ratio']:.2f}%") print(f"移除: {ror_metrics['removed_points']} 点") # 3. 直通滤波 (PF) print("\n" + "=" * 50) print("直通滤波 (PF)") print("=" * 50) points = np.asarray(pcd_noisy.points) z_min, z_max = points[:, 2].min(), points[:, 2].max() pf_filtered, pf_metrics = pass_through_filter(pcd_noisy, axis='z', min_val=z_min, max_val=z_max) results['PF'] = pf_metrics print(f"保留率: {pf_metrics['keep_ratio']:.2f}%") keep_ratio, density_change = compute_keep_ratio_and_density(pcd_noisy, pf_filtered) print(f"密度变化: {density_change:+.2f}%") # 4. 体素滤波 (VGF) print("\n" + "=" * 50) print("体素滤波 (VGF)") print("=" * 50) vgf_filtered, vgf_metrics = voxel_grid_filter(pcd_noisy, leaf_size=args.voxel_size) results['VGF'] = vgf_metrics print(f"保留率: {vgf_metrics['keep_ratio']:.2f}%") print(f"压缩比: {vgf_metrics['compression_ratio']:.1f}x") # 5. 双边滤波 (BF) print("\n" + "=" * 50) print("双边滤波 (BF)") print("=" * 50) bf_filtered, bf_metrics = bilateral_filter(pcd_noisy, sigma_s=args.bf_sigma_s, sigma_r=args.bf_sigma_r) results['BF'] = bf_metrics print(f"保留率: {bf_metrics['keep_ratio']:.2f}%") # Chamfer Distance 计算 print("\n" + "=" * 50) print("Chamfer Distance 评估 (与干净点云对比)") print("=" * 50) for name, metrics in results.items(): if name == 'BF': filtered = bf_filtered elif name == 'SOR': filtered = sor_filtered elif name == 'ROR': filtered = ror_filtered elif name == 'PF': filtered = pf_filtered elif name == 'VGF': filtered = vgf_filtered else: continue if len(filtered.points) > 0: cd, _, _ = chamfer_distance(pcd_clean, filtered) print(f"{name}: CD = {cd:.6f} m") print("-" * 40) cd_noisy, _, _ = chamfer_distance(pcd_clean, pcd_noisy) print(f"加噪点云 (未经滤波): CD = {cd_noisy:.6f} m") # 可视化 if not args.no_visualize: print("\n可视化对比...") pcd_clean.paint_uniform_color([0, 0, 1]) # 蓝色 - 原始干净点云 pcd_noisy.paint_uniform_color([1.0, 0.7, 0.5]) # 浅橙色 - 加噪点云 sor_filtered.paint_uniform_color([0, 1, 0]) # 绿色 - SOR 结果 o3d.visualization.draw_geometries( [pcd_clean, pcd_noisy], window_name=f"Clean (Blue, {len(pcd_clean.points)}) vs Noisy (Orange, {len(pcd_noisy.points)})" ) o3d.visualization.draw_geometries( [pcd_clean, sor_filtered], window_name=f"SOR: Clean (Blue, {len(pcd_clean.points)}) vs Filtered (Green, {len(sor_filtered.points)})" ) if __name__ == "__main__": main()