a目录
1、点到点（point2point）的配准
2、 点到面（point2plane）的配准
3、基于颜色的配准(color-icp)
4、点云配准核函数(robust kernel)
前面已经介绍过点云配准的基础理论内容，可以查看之前的文章：
『OPEN3D』1.8 点云的配准理论-CSDN博客
1、点到点（point2point）的配准
点到点的配准，最小化如下误差函数
代码如下：
import open3d as o3d
import numpy as np
import copy
# 点到点的icp
def point_to_point_icp(source, target, threshold, trans_init):
print("Apply point-to-point ICP")
reg_p2p = o3d.pipelines.registration.registration_icp(
source = source,#原始点云
target = target,#目标点云
# 原始点云与目标点云用于匹配的最大点对距离，小于该距离的才被认为是需要优化的点对
max_correspondence_distance=threshold,
init=trans_init,#初始变换矩阵
#TransformationEstimationPointToPoint 类提供了计算残差和点到点ICP的Jacobian矩阵的函数
estimation_method = o3d.pipelines.registration.TransformationEstimationPointToPoint()
)
print(reg_p2p)
# 输出配准的变换矩阵
print("Transformation is:")
print(reg_p2p.transformation, "\n")
# 绘制结果
draw_registration_result(source, target, reg_p2p.transformation)
if __name__ == "__main__":
pcd_data = o3d.data.DemoICPPointClouds()
# 读取两份点云文件
source = o3d.io.read_point_cloud(pcd_data.paths[0])
target = o3d.io.read_point_cloud(pcd_data.paths[1])
threshold = 0.04
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
[-0.139, 0.967, -0.215, 0.7],
[0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
draw_registration_result(source, target, trans_init)
print("Initial alignment")
# evaluate_registration函数评估了配准的两个参数：
# 1、fitness评估了重叠面积（内点对/目标点云数量），越高越好
# 2、inlier_rmse评估了所有内点对RMSE，数值越低越好
evaluation = o3d.pipelines.registration.evaluate_registration(
source, target, threshold, trans_init)
print(evaluation, "\n")
point_to_point_icp(source, target, threshold, trans_init)
注：通过evaluate_registration函数来判断初始的配准情况
evaluate_registration函数评估了配准的两个参数：
1、fitness评估了重叠面积（内点对/目标点云数量），越高越好
2、inlier_rmse评估了所有内点对RMSE，数值越低越好
输出结果：
可以看到fitness有提升且inlier_rmse有下降，并且匹配的点对数量增加；然后输出匹配的变换矩阵
Initial alignment
RegistrationResult with fitness=3.236251e-01, inlier_rmse=2.185569e-02, and correspondence_set size of 64348
Access transformation to get result.
Apply point-to-point ICP
RegistrationResult with fitness=6.403400e-01, inlier_rmse=8.354068e-03, and correspondence_set size of 127322
Access transformation to get result.
Transformation is:
[[ 0.84051756 0.00711097 -0.54198802 0.64482424]
[-0.1500491 0.96427647 -0.21978666 0.82019636]
[ 0.52014373 0.26524577 0.81144428 -1.48547754]
[ 0. 0. 0. 1. ]]
2、 点到面（point2plane）的配准
点到面的配准，最小化如下误差函数：
其中为点p的法线；并且点到面的ICP比点到点的ICP拥有更快的收敛速度。
点到面的配准需要目标点云具有法线信息,若没有则需要先对目标点云进行法线估计
estimate_normals(
search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))代码如下：
# 点到面的icp
def point_to_plane_icp(source, target, threshold, trans_init):
print("Apply point-to-plane ICP")
reg_p2l = o3d.pipelines.registration.registration_icp(
source=source, # 原始点云
target=target, # 目标点云
# 原始点云与目标点云用于匹配的最大点对距离，小于该距离的点对才被认为是需要优化的点对
max_correspondence_distance=threshold,
init=trans_init, # 初始变换矩阵
# TransformationEstimationPointToPlane 类提供了计算残差和点到面ICP的Jacobian矩阵的函数
estimation_method = o3d.pipelines.registration.TransformationEstimationPointToPlane())
print(reg_p2l)
# 输出配准的变换矩阵
print("Transformation is:")
print(reg_p2l.transformation, "\n")
# 绘制结果
draw_registration_result(source, target, reg_p2l.transformation)
if __name__ == "__main__":
pcd_data = o3d.data.DemoICPPointClouds()
# 读取两份点云文件
source = o3d.io.read_point_cloud(pcd_data.paths[0])
target = o3d.io.read_point_cloud(pcd_data.paths[1])
# 原始点云与目标点云用于匹配的最大点对距离，小于该距离的点对才被认为是需要优化的点对
threshold = 0.04
# 这里人为指定一个初始的变换矩阵，
# 后续我们将使用点云特征匹配的方式来获取初始的变换矩阵
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
[-0.139, 0.967, -0.215, 0.7],
[0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
draw_registration_result(source, target, trans_init)
print("Initial alignment")
# evaluate_registration函数评估了配准的两个参数：
# 1、fitness评估了重叠面积（内点对/目标点云数量），越高越好
# 2、inlier_rmse评估了所有内点对RMSE，数值越低越好
evaluation = o3d.pipelines.registration.evaluate_registration(
source, target, threshold, trans_init)
print(evaluation, "\n")
point_to_plane_icp(source, target, threshold, trans_init)
点到面的匹配结果如下
Initial alignment
RegistrationResult with fitness=3.236251e-01, inlier_rmse=2.185569e-02, and correspondence_set size of 64348
Access transformation to get result.
Apply point-to-plane ICP
RegistrationResult with fitness=6.400634e-01, inlier_rmse=8.221662e-03, and correspondence_set size of 127267
Access transformation to get result.
Transformation is:
[[ 0.84038344 0.00645131 -0.54220491 0.64577952]
[-0.14771349 0.96522059 -0.21719886 0.81064328]
[ 0.52102822 0.2618064 0.81199599 -1.48292341]
[ 0. 0. 0. 1. ]] 3、基于颜色的配准(color-icp)
前面两个算法只是对点云数据进行几何形状的配准，如果是两份平面数据有不同的颜色信息，则不发匹配颜色部分，如下所示：
两份带有不同颜色图案的平面点云
color icp的优化误差函数如下
其中部分与点到面的ICP相同，
部分为photometric误差，
为平衡误差之间的权重；
的误差部分如下：
其中代表了在P切平面的预计算函数，
为投影函数，将一个3d点投影到切面中；
import open3d as o3d
import numpy as np
import copy
# 画图函数
def draw_registration_result(source, target, transformation):
source_temp = copy.deepcopy(source)
source_temp.transform(transformation)
o3d.visualization.draw_geometries([source_temp, target])
# 在此处加载两份点云数据
print("Load two point clouds and show initial pose ...")
ply_data = o3d.data.DemoColoredICPPointClouds()
source = o3d.io.read_point_cloud(ply_data.paths[0])
target = o3d.io.read_point_cloud(ply_data.paths[1])
if __name__ == "__main__":
# 给定初始的变换矩阵
current_transformation = np.identity(4)
# 可视化初始两份点云数据
draw_registration_result(source, target, current_transformation)
print(current_transformation)
# 对点云进行由粗到精的迭代配准
# 对点云进行的下采样voxel大小，单位米
voxel_radius = [0.04, 0.02, 0.01]
# 每层最大迭代次数
max_iter = [50, 30, 14]
# 给定初始的变换矩阵
current_transformation = np.identity(4)
print("Colored point cloud registration ...\n")
for scale in range(3):
iter = max_iter[scale]
radius = voxel_radius[scale]
print([iter, radius, scale])
print("1. 下采样点云，voxel大小为 %.2f" % radius)
source_down = source.voxel_down_sample(radius)
target_down = target.voxel_down_sample(radius)
print("2. 估计点云的法线信息")
source_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
target_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
# 使用coloricp进行配准
print("3. 使用color icp进行配准")
result_icp = o3d.pipelines.registration.registration_colored_icp(
source=source_down, # 原始点云
target=target_down, # 目标点云
# 原始点云与目标点云用于匹配的最大点对距离，小于该距离的点对才被认为是需要优化的点对
max_correspondence_distance=radius,
init=current_transformation, # 初始变换矩阵
# TransformationEstimationForColoredICP 类提供了计算残差和点到面ICP的Jacobian矩阵的函数
# 其中lambda_geometric是前面所说的颜色误差函数调整系数
estimation_method=
o3d.pipelines.registration.TransformationEstimationForColoredICP(lambda_geometric=0.9),
# 迭代条件，达到其中一个则退出当前迭代
criteria=o3d.pipelines.registration.ICPConvergenceCriteria(
relative_fitness=1e-6, relative_rmse=1e-6, max_iteration=iter)
)
# # 使用point2plane进行配准
# print("3. 使用point 2 plane icp进行配准")
# result_icp = o3d.pipelines.registration.registration_icp(
# source=source, # 原始点云
# target=target, # 目标点云
# # 原始点云与目标点云用于匹配的最大点对距离，小于该距离的点对才被认为是需要优化的点对
# max_correspondence_distance=radius * 2,
# init=current_transformation, # 初始变换矩阵
# # TransformationEstimationPointToPlane 类提供了计算残差和点到面ICP的Jacobian矩阵的函数
# estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane())
# 取出当前轮次的配准结果
current_transformation = result_icp.transformation
print(result_icp, "\n")
# 可视化结果
draw_registration_result(source, target, result_icp.transformation)
print(current_transformation)
使用point2plane的配准结果，两份点云的颜色信息不能配准
使用color icp的结果, 两份点云的颜色信息可以良好的配准
4、点云配准核函数(robust kernel)
若点云数据中具有大量的噪声，则前述的方法可能不能得到正确的结果，因此此处引入核函数来对噪声更加泛化。
优化问题中，通常将最小化误差项的二范数平方和作为目标函数；但存在一个严重的问题:如果出于误匹配等原因，某个误差项给的数据是错误的，那么它的梯度也很大，意味着调整与它相关的变量会使目标函数下降更多。所以，算法将试图优先向这个误差大的outlier项进行调整，使其他正确的匹配向满足该项的无理要求，导致算法会抹平其他正确边的影响， 使优化算法专注于调整一个错误的值。这显然不是我们希望看到的。
出现这种问题的原因是，当误差很大时，二范数增长得太快。于是就有了核函数的存在。核 函数保证每条边的误差不会大得没边而掩盖其他的边。具体的方式是，把原先误差的二范数度量 替换成一个增长没有那么快的函数，同时保证自己的光滑'性质(不然无法求导)。因为它们使得 整个优化结果更为稳健，所以又叫它们鲁棒核函数( Robust Kemel )。
鲁棒核函数有许多种，例如最常用的 Huber 核:
除了 Huber 核，还有 Cauchy 核、 Tukey 核，在Open3D中实现了Turky核函数，下面看看roubust icp的误差函数
当前open3d中只有PointToPlane的ICP方法已经实现了核函数，只需要通过在TransformationEstimationPointToPlane的estimation_method方法中加入核函数即可,
TransormationEstimationPointToPlane(loss)内部实现了带权重的残差计算节点到面的ICP的基于给定核函数的Jacobian矩阵实现
import open3d as o3d
import numpy as np
import copy
#画图函数
def draw_registration_result(source, target, transformation):
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
source_temp.paint_uniform_color([1, 0.706, 0])
target_temp.paint_uniform_color([0, 0.651, 0.929])
source_temp.transform(transformation)
o3d.visualization.draw_geometries([source_temp, target_temp])
#对点云添加高斯噪音
def apply_noise(pcd, mu, sigma):
noisy_pcd = copy.deepcopy(pcd)
points = np.asarray(noisy_pcd.points)
# mu为高斯噪音的均值，sigma为方差
points += np.random.normal(mu, sigma, size=points.shape)
# 将生成噪音添加到每个点云上
noisy_pcd.points = o3d.utility.Vector3dVector(points)
return noisy_pcd
if __name__ == "__main__":
pcd_data = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(pcd_data.paths[0])
target = o3d.io.read_point_cloud(pcd_data.paths[1])
# 给定初始变换矩阵
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
[-0.139, 0.967, -0.215, 0.7],
[0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
# Mean and standard deviation.
mu, sigma = 0, 0.05
source_noisy = apply_noise(source, mu, sigma)
print("可视化带有噪音的点云数据:")
o3d.visualization.draw_geometries([source_noisy])
print("可视化初始变换下的原始源点云和目标点云：")
draw_registration_result(source, target, trans_init)
threshold = 1.0
print("Robust point-to-plane ICP, 点到面的阈值={}:".format(threshold))
# 创建核函数，并将参数k设置的与高斯噪音的方差一致, 不同的核函数
# loss = o3d.pipelines.registration.TukeyLoss(k=sigma)
# loss = o3d.pipelines.registration.GMLoss(k=sigma)
# loss = o3d.pipelines.registration.L1Loss()
loss = o3d.pipelines.registration.L2Loss()
print("使用的核函数为:", loss)
# 可以去掉estimation_method中的核函数，看看配准的结果
p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane(loss)
reg_p2l = o3d.pipelines.registration.registration_icp(
source_noisy,
target,
threshold,
trans_init,
p2l)
print(reg_p2l)
print("NNNNNathan 变换矩阵为:")
print(reg_p2l.transformation)
# 可视化结果
draw_registration_result(source, target, reg_p2l.transformation)
添加噪声后的原始点云数据
不使用核函数的配准结果
使用核函数的配准结果
