import copy import argparse import os from scipy.spatial.transform import Rotation as R import trimesh import numpy as np import open3d as o3d import time def rotation_matrix_scipy(roll_deg=10, pitch_deg=10, yaw_deg=10): """ 使用 scipy 生成绕 XYZ 轴旋转的矩阵 """ r = R.from_euler('xyz', [roll_deg, pitch_deg, yaw_deg], degrees=True) return r.as_matrix() def random_rotation_matrix(): """生成随机旋转矩阵""" angle = np.random.uniform(0, 2 * np.pi) axis = np.random.randn(3) axis = axis / np.linalg.norm(axis) K = np.array([ [0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0] ]) R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * K @ K return R def random_translation_vector(range_xyz=0.5): """生成随机平移向量""" return np.random.uniform(-range_xyz, range_xyz, 3) def transform_point_cloud(pcd, R, t): """对点云应用变换""" transformed_pcd = copy.deepcopy(pcd) points = np.asarray(transformed_pcd.points) points_transformed = (R @ points.T).T + t transformed_pcd.points = o3d.utility.Vector3dVector(points_transformed) return transformed_pcd def registration_open3d(source_pcd, target_pcd, voxel_size=0.005): """ 点云配准（RANSAC + ICP） Returns: transformation: 4x4 变换矩阵 fitness: 配准 fitness 分数 rmse: 配准 RMSE """ # 1. 下采样 source_down = source_pcd.voxel_down_sample(voxel_size) target_down = target_pcd.voxel_down_sample(voxel_size) # 2. 估计法线 radius = voxel_size * 2 source_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30)) target_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30)) # 3. 计算 FPFH 特征 radius_feature = voxel_size * 5 source_fpfh = o3d.pipelines.registration.compute_fpfh_feature( source_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) target_fpfh = o3d.pipelines.registration.compute_fpfh_feature( target_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) # 4. RANSAC 粗配准 distance_threshold = voxel_size * 1.5 result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching( source_down, target_down, source_fpfh, target_fpfh, mutual_filter=True, max_correspondence_distance=distance_threshold, estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(), ransac_n=3, checkers=[ o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9), o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold) ], criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999)) print(f"RANSAC 配准完成") source_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) target_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) # 5. ICP 精配准 result_icp = o3d.pipelines.registration.registration_icp( source_pcd, target_pcd, distance_threshold / 2, result_ransac.transformation, o3d.pipelines.registration.TransformationEstimationPointToPlane(), o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=200)) print(f"ICP 精配准完成") print(f" Fitness: {result_icp.fitness:.4f}") print(f" RMSE: {result_icp.inlier_rmse:.6f} m") return result_icp.transformation, result_icp.fitness, result_icp.inlier_rmse def visualize_registration_result(original_pcd, transformed_pcd, registered_pcd): """可视化原始点云、变换后点云和配准后点云""" source_colored = copy.deepcopy(original_pcd) target_colored = copy.deepcopy(transformed_pcd) registered_colored = copy.deepcopy(registered_pcd) source_colored.paint_uniform_color([0, 0, 1]) # 蓝色 - 原始点云 target_colored.paint_uniform_color([0, 1, 0]) # 绿色 - 目标点云 registered_colored.paint_uniform_color([1, 0, 0]) # 红色 - 配准后点云 o3d.visualization.draw_geometries( [source_colored, target_colored], window_name="Before Registration (Blue: Source, Green: Target)", width=1024, height=768 ) o3d.visualization.draw_geometries( [registered_colored, target_colored], window_name="After Registration (Red: Registered, Green: Target)", width=1024, height=768 ) def visualize_registration_comparison(original_pcd, transformed_pcd, registered_pcd): """并排可视化配准前后的对比""" source_before = copy.deepcopy(original_pcd) source_before.paint_uniform_color([0, 0, 1]) # 蓝色 target_before = copy.deepcopy(transformed_pcd) target_before.paint_uniform_color([0, 1, 0]) # 绿色 registered_after = copy.deepcopy(registered_pcd) registered_after.paint_uniform_color([1, 0, 0]) # 红色 o3d.visualization.draw_geometries( [source_before, target_before], window_name="Before Registration (Blue: Source, Green: Target)", width=1024, height=768 ) o3d.visualization.draw_geometries( [registered_after, target_before], window_name="After Registration (Red: Registered, Green: Target)", width=1024, height=768 ) def compute_point_to_mesh_distance_trimesh(points, mesh_path): """使用 trimesh 计算点云到 Mesh 的距离""" mesh_trimesh = trimesh.load(mesh_path) closest_points, distances, triangle_ids = mesh_trimesh.nearest.on_surface(points) return distances # ========== 主程序 ========== def main(): parser = argparse.ArgumentParser(description='点云配准与 Mesh 距离计算') parser.add_argument('--ply_path', '-p', type=str, default="./High_Speed_Data/Noise_PointCloud/11_noisy.ply", help='输入点云文件路径 (.ply)') parser.add_argument('--mesh_path', '-m', type=str, default="./High_Speed_Data/Mesh/11.stl", help='输入 Mesh 文件路径 (.stl, .ply, .obj 等)') parser.add_argument('--voxel_size', '-v', type=float, default=10, help='体素滤波大小 (默认: 10)') parser.add_argument('--roll', type=float, default=10, help='绕 X 轴旋转角度 (度, 默认: 10)') parser.add_argument('--pitch', type=float, default=10, help='绕 Y 轴旋转角度 (度, 默认: 10)') parser.add_argument('--yaw', type=float, default=10, help='绕 Z 轴旋转角度 (度, 默认: 10)') parser.add_argument('--trans_range', type=float, default=0, help='平移范围 (米, 默认: 0)') 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 if not os.path.exists(args.mesh_path): print(f"错误: Mesh 文件不存在 - {args.mesh_path}") return # 1. 读取原始点云 print(f"读取点云: {args.ply_path}") source_pcd = o3d.io.read_point_cloud(args.ply_path) if len(source_pcd.points) == 0: print("错误: 点云读取失败") return print(f"原始点云点数: {len(source_pcd.points)}") # 2. 生成变换 R = rotation_matrix_scipy(args.roll, args.pitch, args.yaw) t = random_translation_vector(range_xyz=args.trans_range) print(f"旋转矩阵:\n{R}") print(f"平移向量: {t}") # 3. 创建目标点云（变换后的源点云） target_pcd = transform_point_cloud(source_pcd, R, t) # 4. 点云配准 print("\n开始点云配准...") t0 = time.time() transformation, fitness, rmse = registration_open3d(source_pcd, target_pcd, voxel_size=args.voxel_size) # 5. 将目标点云变换回源点云坐标系 inv_transformation = np.linalg.inv(transformation) target_recovered = copy.deepcopy(target_pcd) target_recovered.transform(inv_transformation) t1 = time.time() print(f"\n配准结果:") print(f" Fitness: {fitness:.4f}") print(f" RMSE: {rmse:.6f} m") print(f" 估计的变换矩阵:\n{transformation}") print(f"配准耗时: {t1 - t0:.4f} 秒") # 6. 计算点到 Mesh 的距离 print("\n计算点到 Mesh 距离...") t2 = time.time() points = np.asarray(target_recovered.points) distances = compute_point_to_mesh_distance_trimesh(points, args.mesh_path) t3 = time.time() print(f"距离统计:") print(f" 平均距离: {np.mean(distances):.6f} mm ({np.mean(distances) * 1000:.3f} mm)") print(f" 最大距离: {np.max(distances):.6f} mm ({np.max(distances) * 1000:.3f} mm)") print(f" RMSE: {np.sqrt(np.mean(distances ** 2)):.6f} m") print(f"距离计算耗时: {t3 - t2:.4f} 秒") # 7. 可视化 if not args.no_visualize: print("\n开始可视化...") visualize_registration_comparison(source_pcd, target_pcd, target_recovered) if __name__ == "__main__": main()