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