# import numpy as np # import open3d as o3d # import random # # # def ransac_plane(points, threshold=0.01, max_iterations=1000, min_inliers=100): # """ # RANSAC 平面拟合（单个） # """ # best_inliers = [] # best_plane = None # n_points = len(points) # # if n_points < 3: # return None, [] # # for _ in range(max_iterations): # sample_idx = random.sample(range(n_points), 3) # p1, p2, p3 = points[sample_idx] # # v1 = p2 - p1 # v2 = p3 - p1 # normal = np.cross(v1, v2) # norm = np.linalg.norm(normal) # if norm < 1e-8: # continue # normal = normal / norm # a, b, c = normal # d = -np.dot(normal, p1) # # distances = np.abs(np.dot(points, normal) + d) # inliers = np.where(distances < threshold)[0] # # if len(inliers) > len(best_inliers) and len(inliers) >= min_inliers: # best_inliers = inliers # best_plane = (a, b, c, d) # # if len(best_inliers) > 0.9 * n_points: # break # # # 用内点重新拟合 # if best_inliers is not None and len(best_inliers) > 3: # inlier_points = points[best_inliers] # centroid = np.mean(inlier_points, axis=0) # _, _, vh = np.linalg.svd(inlier_points - centroid) # normal = vh[2] # normal = normal / np.linalg.norm(normal) # a, b, c = normal # d = -np.dot(normal, centroid) # best_plane = (a, b, c, d) # # return best_plane, best_inliers # # # def sequential_ransac_planes(points, threshold=0.01, max_iterations=1000, min_inliers=100, max_planes=5): # """ # 迭代 RANSAC 拟合多个平面 # """ # remaining_points = points.copy() # remaining_indices = np.arange(len(points)) # # planes = [] # inliers_list = [] # # for i in range(max_planes): # if len(remaining_points) < min_inliers: # break # # plane, inliers = ransac_plane(remaining_points, threshold, max_iterations, min_inliers) # # if plane is None or len(inliers) < min_inliers: # break # # planes.append(plane) # original_indices = remaining_indices[inliers] # inliers_list.append(original_indices) # # # 移除内点 # mask = np.ones(len(remaining_points), dtype=bool) # mask[inliers] = False # remaining_points = remaining_points[mask] # remaining_indices = remaining_indices[mask] # # print(f"平面 {len(planes)}: {len(inliers)} 个内点, 剩余 {len(remaining_points)} 个点") # # return planes, inliers_list # # # def create_plane_mesh(plane, bounds): # """ # 创建平面网格用于可视化 # """ # a, b, c, d = plane # x_min, x_max = bounds[0] # y_min, y_max = bounds[1] # # x = np.linspace(x_min, x_max, 50) # y = np.linspace(y_min, y_max, 50) # X, Y = np.meshgrid(x, y) # # if abs(c) > 1e-8: # Z = -(a * X + b * Y + d) / c # else: # Z = np.zeros_like(X) # # vertices = [] # triangles = [] # rows, cols = X.shape # # for i in range(rows): # for j in range(cols): # vertices.append([X[i, j], Y[i, j], Z[i, j]]) # # for i in range(rows - 1): # for j in range(cols - 1): # idx = i * cols + j # triangles.append([idx, idx + 1, idx + cols]) # triangles.append([idx + 1, idx + cols + 1, idx + cols]) # # mesh = o3d.geometry.TriangleMesh() # mesh.vertices = o3d.utility.Vector3dVector(np.array(vertices)) # mesh.triangles = o3d.utility.Vector3iVector(np.array(triangles)) # mesh.compute_vertex_normals() # # return mesh # # # def visualize_multiple_planes(points, planes, inliers_list): # """ # 可视化多个平面（点云 + 平面网格） # # Args: # points: (N, 3) 点云坐标 # planes: 平面方程列表 [(a,b,c,d), ...] # inliers_list: 每个平面对应的内点索引列表 # """ # # 颜色列表（每个平面不同颜色） # colors = [ # [1, 0, 0], # 红色 # [0, 1, 0], # 绿色 # [0, 0, 1], # 蓝色 # [1, 1, 0], # 黄色 # [1, 0, 1], # 品红 # [0, 1, 1], # 青色 # [1, 0.5, 0], # 橙色 # [0.5, 0, 1], # 紫色 # ] # # geometries = [] # # # 1. 为每个平面的内点着色 # point_colors = np.zeros((len(points), 3)) # for i, inliers in enumerate(inliers_list): # color = colors[i % len(colors)] # point_colors[inliers] = color # # # 未分配的点（剩余点）设为灰色 # unassigned = np.where(np.sum(point_colors, axis=1) == 0)[0] # point_colors[unassigned] = [0.5, 0.5, 0.5] # # colored_pcd = o3d.geometry.PointCloud() # colored_pcd.points = o3d.utility.Vector3dVector(points) # colored_pcd.colors = o3d.utility.Vector3dVector(point_colors) # geometries.append(colored_pcd) # # # 2. 为每个平面创建网格 # for i, (plane, inliers) in enumerate(zip(planes, inliers_list)): # # 计算该平面对应点的范围 # inlier_points = points[inliers] # if len(inlier_points) > 0: # x_min, x_max = inlier_points[:, 0].min(), inlier_points[:, 0].max() # y_min, y_max = inlier_points[:, 1].min(), inlier_points[:, 1].max() # # # 稍微扩展边界 # x_range = x_max - x_min # y_range = y_max - y_min # x_min -= x_range * 0.1 # x_max += x_range * 0.1 # y_min -= y_range * 0.1 # y_max += y_range * 0.1 # # mesh = create_plane_mesh(plane, [[x_min, x_max], [y_min, y_max]]) # mesh.paint_uniform_color(colors[i % len(colors)]) # # 设置半透明 # mesh.paint_uniform_color([colors[i % len(colors)][0], # colors[i % len(colors)][1], # colors[i % len(colors)][2]]) # geometries.append(mesh) # # # 可视化 # o3d.visualization.draw_geometries( # geometries, # window_name="Multiple Planes - RANSAC", # width=1024, # height=768 # ) # # # def visualize_single_plane(points, plane, inliers): # """ # 可视化单个平面（兼容单平面情况） # """ # visualize_multiple_planes(points, [plane], [inliers]) # # # # ========== 主程序 ========== # if __name__ == "__main__": # # 读取点云 # file_path = r"E:\point_distance\data\part_car_head.ply" # pcd = o3d.io.read_point_cloud(file_path) # # if len(pcd.points) == 0: # print("读取失败，请检查文件路径") # exit() # # points = np.asarray(pcd.points) # print(f"原始点云点数: {len(points)}") # # # 计算点云范围 # points_range = points.max(axis=0) - points.min(axis=0) # print(f"点云范围: X[{points_range[0]:.3f}], Y[{points_range[1]:.3f}], Z[{points_range[2]:.3f}]") # # # 设置阈值 # threshold = points_range[2] * 0.02 # print(f"使用阈值: {threshold:.4f}") # # # ========== 单平面拟合 ========== # # print("\n" + "=" * 50) # # print("单平面拟合") # # print("=" * 50) # # # # plane, inliers = ransac_plane(points, threshold=threshold, max_iterations=1, min_inliers=100) # # # # if plane is not None: # # a, b, c, d = plane # # print(f"拟合平面: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0") # # print(f"内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)") # # visualize_single_plane(points, plane, inliers) # # # ========== 多平面拟合 ========== # print("\n" + "=" * 50) # print("多平面拟合") # print("=" * 50) # # planes, inliers_list = sequential_ransac_planes( # points, # threshold=threshold, # max_iterations=1, # min_inliers=100, # max_planes=1 # ) # # print(f"共提取 {len(planes)} 个平面") # for i, (plane, inliers) in enumerate(zip(planes, inliers_list)): # a, b, c, d = plane # print(f"平面 {i + 1}: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0, 内点数: {len(inliers)}") # # if len(planes) > 0: # visualize_multiple_planes(points, planes, inliers_list) import numpy as np import open3d as o3d import random def create_square_plane_with_noise(size_mm=1000, noise_std_mm=0.5, num_points=10000): """创建正方形平面点云并添加高斯噪声""" size_m = size_mm / 1000.0 noise_std_m = noise_std_mm / 1000.0 half_size = size_m / 2 x = np.random.uniform(-half_size, half_size, num_points) y = np.random.uniform(-half_size, half_size, num_points) z = np.zeros(num_points) points = np.column_stack([x, y, z]) noise_z = np.random.normal(0, noise_std_m, num_points) points[:, 2] += noise_z return points def estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000): """使用 Open3D 内置 RANSAC 平面估计""" pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(points) threshold_m = threshold_mm / 1000.0 plane_model, inliers = pcd.segment_plane( distance_threshold=threshold_m, ransac_n=3, num_iterations=max_iterations ) a, b, c, d = plane_model # 计算误差 ideal_normal = np.array([0, 0, 1]) estimated_normal = np.array([a, b, c]) cos_angle = np.dot(ideal_normal, estimated_normal) cos_angle = np.clip(cos_angle, -1, 1) angle_error = np.degrees(np.arccos(cos_angle)) offset_error = abs(d) inlier_points = points[inliers] distances = np.abs(np.dot(inlier_points, estimated_normal) + d) rmse = np.sqrt(np.mean(distances ** 2)) print(f"\nRANSAC 平面拟合结果 (Open3D):") print(f" 估计平面: {a:.6f}x + {b:.6f}y + {c:.6f}z + {d:.6f} = 0") print(f" 内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)") print(f" 法线角度误差: {angle_error:.4f} 度") print(f" 偏移误差: {offset_error:.6f} m ({offset_error * 1000:.3f} mm)") print(f" 平面拟合 RMSE: {rmse:.6f} m ({rmse * 1000:.3f} mm)") return plane_model, inliers def visualize_plane_fitting(points, plane): """可视化原始点云和拟合平面""" # 原始点云 pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(points) pcd.paint_uniform_color([0.6, 0.8, 1.0]) # 创建平面点云 a, b, c, d = plane x_min, x_max = points[:, 0].min(), points[:, 0].max() y_min, y_max = points[:, 1].min(), points[:, 1].max() x = np.linspace(x_min, x_max, 50) y = np.linspace(y_min, y_max, 50) X, Y = np.meshgrid(x, y) if abs(c) > 1e-8: Z = -(a * X + b * Y + d) / c else: Z = np.zeros_like(X) plane_points = np.column_stack([X.ravel(), Y.ravel(), Z.ravel()]) pcd_plane = o3d.geometry.PointCloud() pcd_plane.points = o3d.utility.Vector3dVector(plane_points) pcd_plane.paint_uniform_color([1, 0, 0]) # 红色 o3d.visualization.draw_geometries( [pcd, pcd_plane], window_name="Plane Fitting", width=1024, height=768 ) # ========== 主程序 ========== if __name__ == "__main__": # 参数设置 size_mm = 1000 noise_std_mm = 0.5 num_points = 100000 print(f"参数:") print(f" 正方形边长: {size_mm} mm") print(f" 噪声标准差: {noise_std_mm} mm") print(f" 点数: {num_points}") # 生成点云 points = create_square_plane_with_noise(size_mm, noise_std_mm, num_points) # 检查数据范围 print(f"\n数据范围:") print(f" X: [{points[:, 0].min():.3f}, {points[:, 0].max():.3f}]") print(f" Y: [{points[:, 1].min():.3f}, {points[:, 1].max():.3f}]") print(f" Z: [{points[:, 2].min():.6f}, {points[:, 2].max():.6f}]") # 使用 Open3D RANSAC plane, inliers = estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000) # 可视化 visualize_plane_fitting(points, plane)