一、主要函数
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、配准结果