基于ICP算法的三维点云配准

1
import numpy as np
2
import open3d as o3d
3
import copy
4

5

6
# 生成示例点云并打乱顺序
7
def generate_sample_point_clouds(n_points=1000):
8
    # 生成随机源点云
9
    source_points = np.random.rand(n_points, 3) * 10
10
    # 打乱源点云顺序
11
    np.random.shuffle(source_points)
12
    source = o3d.geometry.PointCloud()
13
    source.points = o3d.utility.Vector3dVector(source_points)
14
    # 生成目标点云（通过随机变换）
15
    rotation = o3d.geometry.get_rotation_matrix_from_xyz([0.3, 0.2, 0.1])
16
    translation = np.array([1.0, 2.0, 3.0])
17
    target_points = (source_points @ rotation.T) + translation
18
    # 打乱目标点云顺序
19
    np.random.shuffle(target_points)
20
    target = o3d.geometry.PointCloud()
21
    target.points = o3d.utility.Vector3dVector(target_points)
22
    return source, target
23

24

25
# ICP算法实现
26
def icp(source, target, max_iterations=50, threshold=0.01):
27
    """
28
    实现ICP算法
29
    参数:
30
        source: 源点云 (Open3D PointCloud)
31
        target: 目标点云 (Open3D PointCloud)
32
        max_iterations: 最大迭代次数
33
        threshold: 收敛阈值
34
    返回:
35
        transformation: 最终变换矩阵 (4x4)
36
    """
37
    source_points = np.asarray(source.points)
38
    target_points = np.asarray(target.points)
39

40

41
    # 初始化变换矩阵
42
    transformation = np.eye(4)
43
    for i in range(max_iterations):
44
        # 构建KD树以查找最近点
45
        target_tree = o3d.geometry.KDTreeFlann(target)
46
        correspondences = []
47
        # 寻找最近点对
48
        for j, point in enumerate(source_points):
49
            [_, idx, _] = target_tree.search_knn_vector_3d(point, 1)
50
            correspondences.append([j, idx[0]])
51
        # 提取对应点
52
        src_corr = source_points[[c[0] for c in correspondences]]
53
        tgt_corr = target_points[[c[1] for c in correspondences]]
54
        # 计算质心
55
        src_centroid = np.mean(src_corr, axis=0)
56
        tgt_centroid = np.mean(tgt_corr, axis=0)
57
        # 去中心化
58
        src_centered = src_corr - src_centroid
59
        tgt_centered = tgt_corr - tgt_centroid
60
        # 计算协方差矩阵
61
        H = src_centered.T @ tgt_centered
62
        # 使用SVD分解计算旋转矩阵
63
        U, _, Vt = np.linalg.svd(H)
64
        rotation = Vt.T @ U.T
65
        # 计算平移向量
66
        translation = tgt_centroid - rotation @ src_centroid
67
        # 更新变换矩阵
68
        delta_transform = np.eye(4)
69
        delta_transform[:3, :3] = rotation
70
        delta_transform[:3, 3] = translation
71

72

73
        # 应用变换
74
        transformation = delta_transform @ transformation
75
        source_points = (rotation @ source_points.T).T + translation
76
        # 检查收敛
77
        error = np.mean(np.linalg.norm(src_corr - tgt_corr, axis=1))
78
        if error < threshold:
79
            print(f"ICP converged after {i+1} iterations with error {error}")
80
            break
81
    # 更新源点云
82
    source.points = o3d.utility.Vector3dVector(source_points)
83
    return transformation
84

85

86
# 应用变换矩阵进行点云配准
87
def apply_transformation(source, transformation):
88
    """
89
    应用变换矩阵到源点云
90
    参数:
91
        source: 源点云 (Open3D PointCloud)
92
        transformation: 变换矩阵 (4x4)
93
    返回:
94
        transformed: 配准后的点云
95
    """
96
    transformed = copy.deepcopy(source)
97
    transformed.transform(transformation)
98
    return transformed
99

100

101
# 可视化点云
102
def visualize_point_clouds(source, target, transformed, offset=[0.0, 0.0, 0.0]):
103
    """
104
    可视化点云，添加偏移以区分配准后的点云
105
    参数:
106
        source: 原始源点云 (红色)
107
        target: 目标点云 (绿色)
108
        transformed: 配准后的点云 (蓝色)
109
        offset: 配准点云的偏移量，仅用于可视化
110
    """
111
    source = copy.deepcopy(source)
112
    target = copy.deepcopy(target)
113
    transformed = copy.deepcopy(transformed)
114
    # 打印点云点数以调试
115
    print(f"Source point cloud has {len(source.points)} points")
116
    print(f"Target point cloud has {len(target.points)} points")
117
    print(f"Transformed point cloud has {len(transformed.points)} points")
118
    # 应用偏移（仅用于可视化）
119
    if offset != [0.0, 0.0, 0.0]:
120
        transformed_points = np.asarray(transformed.points) + np.array(offset)
121
        transformed.points = o3d.utility.Vector3dVector(transformed_points)
122
    source.paint_uniform_color([1, 0, 0])  # 红色
123
    target.paint_uniform_color([0, 1, 0])  # 绿色
124
    transformed.paint_uniform_color([0, 0, 1])  # 蓝色
125
    # 显示所有点云
126
    o3d.visualization.draw_geometries([source, target, transformed],
127
                                    window_name="ICP Registration with Shuffled Points",
128
                                    width=800, height=600)
129
    # 单独显示配准后的点云和目标点云，便于检查
130
    o3d.visualization.draw_geometries([target, transformed],
131
                                    window_name="Target and Transformed Point Clouds",
132
                                    width=800, height=600)
133

134

135
def main():
136
    # 生成点云并打乱顺序
137
    source, target = generate_sample_point_clouds()
138
    # 复制源点云用于变换
139
    source_copy = copy.deepcopy(source)
140
    # 运行ICP算法
141
    transformation = icp(source_copy, target)
142
    print("Final transformation matrix:\n", transformation)
143
    # 应用变换进行配准
144
    transformed_source = apply_transformation(source, transformation)
145
    # 可视化，添加小偏移以区分蓝色点云
146
    visualize_point_clouds(source, target, transformed_source, offset=[0.1, 0.1, 0.1])
147

148

149
if __name__ == "__main__":
150
    main()