基于Open3D与ICP的马鞍与马背3D点云配准初始位姿优化求助

我们使用Open3D及Iterative Closest Point（ICP）算法实现马鞍与马背的3D点云配准，目前通过设置initial_transformation可将马鞍大致放置在马背上，但运行ICP后马鞍会过度前移。推测是初始位置中马鞍穿透马背过多导致的，现寻求更好的初始变换优化方法。

相关代码如下：

def match_saddle(horse_file_path, saddle_file_path):
    try:
        
        horse_pcd = load_point_cloud(horse_file_path)
        saddle_pcd = load_point_cloud(saddle_file_path)
        
        threshold = 5

        # Find the highest y points in both point clouds
        highest_y_horse = get_highest_y_point(horse_pcd)
        highest_y_saddle = get_highest_y_point(saddle_pcd)
        highest_x_saddle = get_highest_x_point(saddle_pcd)
        
        # Calculate the initial transformation
        initial_transformation = np.eye(4)
        initial_transformation[:3, 3] = [
            highest_y_horse[0] - highest_x_saddle[0], 
            highest_y_horse[1] - highest_y_saddle[1] + 100, 
            highest_y_horse[2] - highest_y_saddle[2]   
        ]

        saddle_pcd_filtered = o3d.geometry.PointCloud()
        saddle_pcd_filtered.points = o3d.utility.Vector3dVector(np.asarray(saddle_pcd.points))
        icp_result = o3d.pipelines.registration.registration_icp(
            saddle_pcd_filtered, horse_pcd, threshold, init = initial_transformation,
            estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane()
        )

        saddle_pcd.transform(icp_result.transformation)
        o3d.visualization.draw_geometries([horse_pcd, saddle_pcd], window_name=saddle_file_path)
        
        return saddle_file_path

优化建议 #

1. 精准控制初始高度，避免穿透 #

当前代码用固定值+100调整Y轴偏移，容易出现过度穿透或位置过高的问题。可以通过统计马鞍点云的实际尺寸计算合理偏移：

• 获取马鞍点云的Y轴最小值（对应马鞍底部）：saddle_y_min = np.min(np.asarray(saddle_pcd.points)[:,1])
• 计算马鞍底部到其最高点的距离：saddle_height = highest_y_saddle[1] - saddle_y_min
• 初始Y轴偏移设为马背最高点Y值减去马鞍底部Y值，再加小间隙（如20）：

  initial_transformation[:3, 3][1] = highest_y_horse[1] - saddle_y_min + 20
  

2. 基于PCA对齐初始姿态 #

仅靠平移的初始变换可能忽略姿态差异，导致ICP为匹配点过度移动。可通过主成分分析（PCA）对齐两者的主方向：

# 计算马背和马鞍的PCA
horse_pca = horse_pcd.compute_principal_components_and_mean()
saddle_pca = saddle_pcd.compute_principal_components_and_mean()

# 获取主轴（第一主成分）
horse_axis = horse_pca.principal_components[:,0]
saddle_axis = saddle_pca.principal_components[:,0]

# 计算旋转矩阵，对齐主轴
v = np.cross(saddle_axis, horse_axis)
c = np.dot(saddle_axis, horse_axis)
s = np.linalg.norm(v)
kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))

# 整合到初始变换矩阵
initial_transformation[:3, :3] = rotation_matrix

3. 使用鲁棒ICP减少异常点影响 #

Point-to-Plane ICP对穿透产生的异常匹配点敏感，可改用带鲁棒核的估计方法：

icp_result = o3d.pipelines.registration.registration_icp(
    saddle_pcd_filtered, horse_pcd, threshold, init=initial_transformation,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlaneWithRobustKernel(
        kernel=o3d.pipelines.registration.TukeyKernel(kernel_parameter=1.0)
    )
)

4. 预过滤穿透点 #

在ICP前过滤掉马鞍点云中处于马背内部的点，避免错误匹配：

# 计算马鞍点到马背点云的距离
distances = saddle_pcd_filtered.compute_point_cloud_distance(horse_pcd)
# 保留距离大于0的点（即不在马背内部的点）
valid_indices = [i for i, d in enumerate(distances) if d > 0]
saddle_pcd_filtered = saddle_pcd_filtered.select_by_index(valid_indices)

内容的提问来源于stack exchange，提问作者TildaM