open3d进行ICP点云配准

一、代码

import numpy as np
import open3d as o3d
from scipy.spatial.transform import Rotation as R

# 1. 加载源点云和目标点云
source = o3d.io.read_point_cloud("bun_zipper.ply")
target = o3d.io.read_point_cloud("bun_zipper2.ply")
source.paint_uniform_color([1, 0, 0])
target.paint_uniform_color([0, 0, 1])
o3d.visualization.draw_geometries([source, target])
# 下采样和法线计算
source = source.voxel_down_sample(0.001)
target = target.voxel_down_sample(0.001)
source.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
target.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

# 2. 点云配准
trans_init = np.eye(4)
reg_p2p = o3d.pipelines.registration.registration_icp(
    source, target, 0.02, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPlane(),
    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=5000)
)

reg_p_trans = reg_p2p.transformation
# 3. 打印相对位姿变化
translation_vector = reg_p_trans[:3, 3]
rotation_matrix = reg_p_trans[:3, :3]
quat = R.from_matrix(rotation_matrix.copy()).as_quat()
# 欧拉角
theta_x = np.arctan2(rotation_matrix[2, 1], rotation_matrix[2, 2])
theta_y = np.arctan2(-rotation_matrix[2, 0], np.sqrt(rotation_matrix[2, 1] ** 2 + rotation_matrix[2, 2] ** 2))
theta_z = np.arctan2(rotation_matrix[1, 0], rotation_matrix[0, 0])
# 弧度转度数
theta_x_deg = np.degrees(theta_x)
theta_y_deg = np.degrees(theta_y)
theta_z_deg = np.degrees(theta_z)
print("Transformation matrix:", np.round(reg_p_trans, 3))
print("Rotation matrix:", np.round(rotation_matrix, 3))
print("Translation vector (x, y, z):", translation_vector)
print("Euler Angle:\n\tx=%2f\n\ty=%2f\n\tz=%2f" %(theta_x_deg, theta_y_deg, theta_z_deg))
print("Quaternion:\n\tw=%2f\n\tx=%2f\n\ty=%2f\n\tz=%2f" %(quat[3], quat[0], quat[1], quat[2]))

# 可视化
source.paint_uniform_color([1, 0, 0])  # 红色为源点云，将其旋转回原始方向
target.paint_uniform_color([0, 1, 0])  # 绿色为目标点云
source.paint_uniform_color([1, 0, 0])
target.paint_uniform_color([0, 0, 1])
source.transform(reg_p_trans)
o3d.visualization.draw_geometries([source, target])  # 展示源点云和目标点云

二、结果

配准前

配准后

打印输出

三、安装

我用的是conda安装

conda install -c open3d-admin open3d

示例文件：链接：https://pan.baidu.com/s/1ql_q4jnUZjlZL3l3fRo8vQ 
提取码：wstc