a目录
[2、 点到面（point2plane）的配准](#2、 点到面（point2plane）的配准)
[4、点云配准核函数(robust kernel)](#4、点云配准核函数(robust kernel))
前面已经介绍过点云配准的基础理论内容，可以查看之前的文章：
1、点到点（point2point）的配准
点到点的配准，最小化如下误差函数
代码如下：
python
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有下降，并且匹配的点对数量增加；然后输出匹配的变换矩阵
bash
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拥有更快的收敛速度。
点到面的配准需要目标点云具有法线信息,若没有则需要先对目标点云进行法线估计
python
estimate_normals(
search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))代码如下：
python
# 点到面的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)
点到面的匹配结果如下
bash
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点投影到切面中；
python
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的误差函数
核函数的主要思想便是通过权重控制等方式降低某项数据带来的过大残差，并认为该数据项是outlier，减轻他对算法的影响；上式中
便是核函数，
是第i项数据的残差。
当前open3d中只有PointToPlane的ICP方法已经实现了核函数，只需要通过在TransformationEstimationPointToPlane的estimation_method方法中加入核函数即可。
TransormationEstimationPointToPlane(loss)内部实现了带权重的残差计算节点到面的ICP的基于给定核函数的Jacobian矩阵实现
python
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)添加噪声后的原始点云数据
不使用核函数的配准结果
使用核函数的配准结果
