基于ICP算法的两组点云对齐失败问题求助

问题描述 #

使用Python的Open3D库结合ICP算法合并两组.ply格式的沙堆点云时，第二组点云未正确对齐，直接重叠到第一组点云上，无法得到完整的沙堆形态。

原始代码 #

import open3d as o3d
import numpy as np

def load_point_clouds(ply_files):
    pcds = []
    for file in ply_files:
        pcd = o3d.io.read_point_cloud(file)
        pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
        pcds.append(pcd)
    return pcds

def pairwise_registration(source, target):
    print("Apply point-to-point ICP")
    icp_coarse = o3d.pipelines.registration.registration_icp(
        source, target, 0.1, np.identity(4),
        o3d.pipelines.registration.TransformationEstimationPointToPoint())
    icp_fine = o3d.pipelines.registration.registration_icp(
        source, target, 0.05, icp_coarse.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPoint())

    print("Apply point-to-plane ICP")
    icp_coarse = o3d.pipelines.registration.registration_icp(
        source, target, 0.1, icp_fine.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane())
    icp_fine = o3d.pipelines.registration.registration_icp(
        source, target, 0.05, icp_coarse.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane())

    transformation_icp = icp_fine.transformation
    information_icp = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
        source, target, 0.05, icp_fine.transformation)
    return transformation_icp, information_icp

def full_registration(pcds):
    pose_graph = o3d.pipelines.registration.PoseGraph()
    odometry = np.identity(4)
    pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))

    for target_id in range(1, len(pcds)):
        transformation_icp, information_icp = pairwise_registration(pcds[0], pcds[target_id])
        odometry = np.dot(transformation_icp, odometry)
        pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(np.linalg.inv(odometry)))
        pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(0, target_id,
                                                                        transformation_icp, information_icp,
                                                                        uncertain=False))
    return pose_graph

    def merge_point_clouds(ply_files):
    pcds = load_point_clouds(ply_files)

    print("Full registration ...")
    pose_graph = full_registration(pcds)

    print("Optimizing PoseGraph ...")
    option = o3d.pipelines.registration.GlobalOptimizationOption(max_correspondence_distance=0.05,
                                                                edge_prune_threshold=0.25,
                                                                reference_node=0)
    o3d.pipelines.registration.global_optimization(pose_graph,
                                                    o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
                                                    o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
                                                option)

print("Transform points and display")
for point_id in range(len(pcds)):
    print(pose_graph.nodes[point_id].pose)
    pcds[point_id].transform(pose_graph.nodes[point_id].pose)

# Rotate the second point cloud by 180 degrees around the Z axis
'''rotation_matrix = np.array([[ -1,  0,  0,  0],
                            [  0, -1,  0,  0],
                            [  0,  0,  1,  0],
                            [  0,  0,  0,  1]])
pcds[1].transform(rotation_matrix)'''

pcd_combined = o3d.geometry.PointCloud()
for pcd in pcds:
    pcd_combined += pcd
pcd_combined_down = pcd_combined.voxel_down_sample(voxel_size=0.005)
o3d.io.write_point_cloud("merged_point_cloud.ply", pcd_combined_down)
print("Merged point cloud saved as merged_point_cloud.ply")

ply_files = [
    "/content/test_0.ply",
    "/content/test_1.ply",
    # Add more point cloud files as needed
]

merge_point_clouds(ply_files)

问题分析 #

1. 代码结构错误：merge_point_clouds函数被错误缩进，成为full_registration的内部函数，全局调用时无法找到该函数；后续变换、合并代码直接在全局作用域执行，此时pcds和pose_graph未定义，实际是未执行任何变换直接合并点云，导致重叠。
2. ICP初始变换缺失：直接用单位矩阵作为ICP初始变换，若两组点云初始位置/姿态差异大，ICP易陷入局部最优，对齐失败。
3. Pose Graph逻辑错误：变换矩阵的累积和节点姿态计算逻辑有误，导致最终应用的变换方向错误。
4. 缺少粗配准：未做基于特征的全局粗配准，无法为ICP提供合适的初始变换。

修复方案 #

修复后代码 #

import open3d as o3d
import numpy as np

def load_point_clouds(ply_files):
    pcds = []
    for file in ply_files:
        pcd = o3d.io.read_point_cloud(file)
        pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
        pcds.append(pcd)
    return pcds

def preprocess_point_cloud(pcd, voxel_size):
    # 下采样并计算FPFH特征
    pcd_down = pcd.voxel_down_sample(voxel_size)
    pcd_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*2, max_nn=30))
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        pcd_down, o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size*5, max_nn=100))
    return pcd_down, pcd_fpfh

def execute_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size):
    # 基于RANSAC的粗配准
    distance_threshold = voxel_size * 1.5
    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh, True,
        distance_threshold,
        o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
        3, [
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold)
        ], o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))
    return result

def refine_registration(source, target, voxel_size, initial_transform):
    # 点到平面ICP细化
    distance_threshold = voxel_size * 0.4
    result = o3d.pipelines.registration.registration_icp(
        source, target, distance_threshold, initial_transform,
        o3d.pipelines.registration.TransformationEstimationPointToPlane())
    return result

def merge_point_clouds(ply_files):
    pcds = load_point_clouds(ply_files)
    voxel_size = 0.01  # 根据点云密度调整

    # 以第一组点云为参考，对齐第二组点云
    source = pcds[1]
    target = pcds[0]

    # 粗配准
    source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
    target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)
    result_ransac = execute_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size)
    print(f"粗配准变换矩阵:\n{result_ransac.transformation}")

    # ICP细化
    result_icp = refine_registration(source, target, voxel_size, result_ransac.transformation)
    print(f"ICP细化后变换矩阵:\n{result_icp.transformation}")
    print(f"ICP拟合误差: {result_icp.fitness:.4f}, RMSE: {result_icp.inlier_rmse:.4f}")

    # 应用变换到源点云
    source.transform(result_icp.transformation)

    # 合并点云
    pcd_combined = o3d.geometry.PointCloud()
    pcd_combined += target
    pcd_combined += source

    # 下采样减少点数量
    pcd_combined_down = pcd_combined.voxel_down_sample(voxel_size=0.005)
    o3d.io.write_point_cloud("merged_point_cloud.ply", pcd_combined_down)
    print("合并后的点云已保存为 merged_point_cloud.ply")

    # 可视化查看结果
    o3d.visualization.draw_geometries([pcd_combined_down])

if __name__ == "__main__":
    ply_files = [
        "/content/test_0.ply",
        "/content/test_1.ply",
        # 可添加更多点云文件
    ]
    merge_point_clouds(ply_files)

关键优化说明 #

• 粗配准+ICP细化：先用FPFH特征匹配结合RANSAC得到全局最优初始变换，再用点到平面ICP做精细对齐，避免ICP陷入局部最优。
• 简化对齐逻辑：两组点云直接以第一组为参考对齐第二组，无需复杂的Pose Graph（多组点云时再考虑全局优化）。
• 代码结构修正：所有逻辑封装在函数内部，避免全局变量未定义问题。
• 参数可调整：voxel_size需根据点云实际密度调整，确保特征计算和配准的准确性。

额外调试建议 #

1. 若对齐仍有问题，先可视化原始点云确认初始位置关系，调整voxel_size和distance_threshold参数。
2. 若两组点云存在已知旋转差异（比如用户注释的Z轴180度旋转），可在粗配准前手动施加初始旋转，再执行配准。
3. 关注ICP的fitness和inlier_rmse指标：fitness越接近1说明匹配点越多，inlier_rmse越小说明配准精度越高。

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