Open3D 点对点的ICP配准算法

一、主要函数

1、该类TransformationEstimationPointToPoint提供用于计算点对点ICP目标函数的残差和雅可比矩阵的函数。函数registration_icp将其作为参数并运行点对点ICP以获得结果。
2、该函数evaluate_registration计算两个主要指标。fitness计算重叠区域（内点对应关系/目标点数）。越高越好。inlier_rmse计算所有内在对应关系的均方根误差RMSE。越低越好。
3、由于函数transformand paint_uniform_color会更改点云，可视化部分调用copy.deepcoy进行复制并保护原始点云。

二、代码实现

import copy
import open3d as o3d
#====================读取点云数据===================
source = o3d.io.read_point_cloud("1.pcd")
target = o3d.io.read_point_cloud("2.pcd")
#==================可视化点云初始位置===============
o3d.visualization.draw_geometries([source, target],width=600,height=600)
threshold = 0.2 #距离阈值
trans_init = o3d.np.asarray([[1.0, 0.0, 0.0, 0.0],
                         [0.0, 1.0, 0.0, 0.0],
                         [0.0, 0.0, 1.0, 0],
                         [0.0, 0.0, 0.0, 1.0]]) #初始变换矩阵，一般由粗配准提供
#=================================================
#计算两个重要指标，fitness计算重叠区域（内点对应关系/目标点数）。越高越好。
#inlier_rmse计算所有内在对应关系的均方根误差RMSE。越低越好。
print("Initial alignment")
evaluation = o3d.registration.evaluate_registration(source, target, threshold, trans_init)
print(evaluation)#这里输出的是初始位置的 fitness和RMSE
print("Apply point-to-point ICP")
icp_p2p = o3d.registration.registration_icp(
        source, target, threshold, trans_init,
        o3d.registration.TransformationEstimationPointToPoint(),#执行点对点的ICP算法
        o3d.registration.ICPConvergenceCriteria(max_iteration=30))#设置最大迭代次数
print(icp_p2p)#输出ICP相关信息
print("Transformation is:")
print(icp_p2p.transformation)#输出变换矩阵
#================可视化配准结果====================
def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)       #由于函数transformand paint_uniform_color会更改点云，
    target_temp = copy.deepcopy(target)       #因此调用copy.deepcoy进行复制并保护原始点云。
    source_temp.paint_uniform_color([1, 0, 0])#点云着色
    target_temp.paint_uniform_color([0, 1, 0])
    source_temp.transform(transformation)
    o3d.io.write_point_cloud("trans_of_source.pcd", source_temp)#保存点云
    o3d.visualization.draw_geometries([source_temp, target_temp],width=600,height=600)
draw_registration_result(source, target, icp_p2p.transformation)

三、结果展示

1、初始位置

2、配准结果

四、参考链接

1、ICP Registration
2、Open3d 学习计划——9（ICP配准）
3、教程：Python Open3d 完成 ICP 点云配准