380 lines
12 KiB
Python
380 lines
12 KiB
Python
# 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) |