Python 进行点云ICP(lterative Closest Point)配准(精配准)
前几节分享了单个点云的处理关系，本节我们分享点云之间的处理关系。点云之间的最常用的关系就是配准，而配准中最古老，也最具代表意义的非ICP方法莫属，所以我们从这个经典的算法开始分享。
ICP算法是英文名字lterative Closest Point的缩写，所以直译是迭代最近点算法。那么从名字我们就能知道算法原理的七七八八了。不错，就是找最近的对应点，不断迭代完成配准。整个ICP算法的流程可以分为以下几个部分：
1）找最近
为源点云中的每个点，在目标点云里搜索距离最近的邻居，形成一一对应的点对。这一步用 KD-Tree 等数据结构可以显著加速。
2）算变换
基于当前对应点对，利用最小二乘或 SVD 求解一个刚体变换：旋转矩阵 R 与平移向量 t，使所有点对之间的距离平方和最小。
3）应用变换
把求得的 R、t 作用到源点云，整体移动一次。
4）判断收敛
若所有点对的平均距离已经小于设定阈值，或连续两次迭代改进很小，或达到最大迭代次数，则停止；否则回到步骤 1 继续下一轮。
算法开始时通常把源点云与目标点云大致放在同一坐标系即可，无需精确初值；每次迭代都在缩小两片点云之间的差异，直至重合或满足终止条件。
本次使用的数据依然是我们的老朋友——兔砸，显示如下：
一、点云ICP配准程序
ICP配准有两种，一种是点对点的，一种是点对面的。一般来说稀疏点云用点对点，稠密点云用点对面，同时点对点配准不需要计算法向量，同学们根据需要修改，两种方式如下：
o3d.pipelines.registration.TransformationEstimationPointToPlane(), # 执行点对面的ICP算法o3d.pipelines.registration.TransformationEstimationPointToPoint(), # 执行点对点的ICP算法
import copy
import open3d as o3d
import numpy as np
# -------------------读取点云数据--------------------
source = o3d.io.read_point_cloud("E:/CSDN/规则点云/bunny.pcd")# 原始点云x轴平移0.1，y轴平移0.15，z轴平移0.2，然后加上噪声，最后新的点云作为目标点云
# 平移
target = copy.deepcopy(source)
points = np.asarray(target.points)
t = np.array([0.1, 0.15, 0.2])
target = target.translate(t, relative=True) # relative=True 表示“增量”平移# 加噪声
mu, sigma = 0, 0.001 # 均值和标准差根据密度调节
random_numbers = np.random.normal(mu, sigma, size=(points.shape[0], 3))
points += random_numbers
target.points = o3d.utility.Vector3dVector(points)target.paint_uniform_color([0, 1, 0]) # 给目标点云赋予单色# --------------------计算法向量---------------------
source.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.01, max_nn=30))
target.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.01, max_nn=30))# 复制点云，防止更改
source1 = copy.deepcopy(source)
target1 = copy.deepcopy(target)# 配置参数
threshold = 1 # 配准点对的最大配准距离阈值
trans_init = np.eye(4) # 这里是初始变换矩阵，一般由粗配准提供，这里使用单位矩阵代替
evaluation = o3d.pipelines.registration.evaluate_registration(source, target, threshold, trans_init)
print("配准前信息：", evaluation) # 这里输出的是初始位置的fitness和RMSE，fitness是覆盖率，代表对应点之间的覆盖程度，RMSE是均方根误差，也是对应点之间的icp_p2plane = o3d.pipelines.registration.registration_icp(source, target, threshold, trans_init,o3d.pipelines.registration.TransformationEstimationPointToPlane(), # 执行点对面的ICP算法o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50)) # 设置最大迭代次数
print("配准后信息：", icp_p2plane) # 输出ICP相关信息
print("两块点云之间的配准矩阵:", icp_p2plane.transformation)source2 = copy.deepcopy(source1)
source2.transform(icp_p2plane.transformation)o3d.visualization.draw_geometries([source1, target1],window_name="两点云初始位置",width=1200, height=800,left=50, top=50)
o3d.visualization.draw_geometries([source2, target1],window_name="配准后两点云初始位置",width=1200, height=800,left=50, top=50)二、点云ICP配准结果
点云配准结果
可看到两只兔子刚开始离的还比较远，经过配准之后就重叠了。不得不说，ICP真的牛批。就酱，下次见^-^