lipan
29f1d45d66
算法更新: delivery v1.2.1、平整度v1.5、测距v2.2、整车配准v1.3 适配器: vehicle-registration 新增、各算法 manifest 和 adapter 更新 后端: taskCadFiles/source 字段迁移、derivedPointClouds 注册、取消任务联动 前端: ElapsedTime 时长显示、两步算法流程、canRun 逻辑修复 文档: 新增5份算法性能分析报告
431 lines
14 KiB
Python
431 lines
14 KiB
Python
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() |