目录

一、概述

1.1核心思想

1.2实现步骤

二、代码实现

2.1关键函数

2.2完整代码

三、实现效果

3.1原始点云

3.2配准后点云

3.3计算数据

---

一、概述

        基于点对面的ICP（Iterative Closest Point）配准算法是ICP的一种变体，它通过最小化源点云中每个点到目标点云表面的距离来实现配准。与传统的点对点ICP算法相比，点对面ICP可以在某些情况下提供更好的配准精度，特别是当目标点云具有复杂的几何形状时。

1.1核心思想

点对面ICP配准算法的核心思想是：

1. 对于源点云中的每个点，找到目标点云中的最近面。
2. 计算源点到该面的垂直距离。
3. 通过最小化这些垂直距离来估计刚体变换（旋转和平移），使源点云与目标点云对齐。

1.2实现步骤

具体步骤如下：

1. 初始化：选择一个初始变换（通常是单位变换）将源点云与目标点云进行初步对齐。
2. 最近面配对：对于源点云中的每个点，找到目标点云中最近的面（通常由三角形网格表示）。这一步通常通过最近邻搜索来实现。
3. 计算误差：计算源点到目标面之间的垂直距离（误差）。
4. 最小化误差：通过最小化这些垂直距离来估计新的刚体变换。
5. 迭代：应用计算得到的刚体变换，并重复以上步骤，直到误差收敛或达到最大迭代次数。

二、代码实现

2.1关键函数

1、该类TransformationEstimationPointToPlane()提供用于计算点对面的ICP目标函数的残差和雅可比矩阵的函数。函数registration_icp将其作为参数并运行点对面的ICP以获得结果。

2、该函数evaluate_registration计算两个主要指标。fitness计算重叠区域（内点对应关系/目标点数）。越高越好。inlier_rmse计算所有内在对应关系的均方根误差RMSE。越低越好。

3、由于函数transformand paint_uniform_color会更改点云，可视化部分调用copy.deepcoy进行复制并保护原始点云。

2.2完整代码

import copy
import open3d as o3d
import numpy as np
# -------------------读取点云数据--------------------
source = o3d.io.read_point_cloud("hand.pcd")
target = o3d.io.read_point_cloud("hand_trans.pcd")
# --------------------计算法向量---------------------
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))
# ----------------可视化点云初始位置-----------------
o3d.visualization.draw_geometries([source, target], width=600, height=600, mesh_show_back_face=False)
threshold = 1 # 距离阈值
trans_init = np.asarray([[ 0.98194534, -0.18295687, -0.04806395,  0.65088957],
                        [ 0.11626176,  0.78413388, -0.60960419,  4.19087836],
                        [ 0.14921985,  0.59300999,  0.79124749,  0.42555584],
                            [ 0,          0,          0,          1        ]])  # 初始变换矩阵，一般由粗配准提供
print("Initial alignment")
evaluation = o3d.pipelines.registration.evaluate_registration(source, target, threshold, trans_init)
print(evaluation)  # 这里输出的是初始位置的 fitness和RMSE
print("Apply point-to-plane ICP")
icp_p2plane = o3d.pipelines.registration.registration_icp(
        source, target, threshold, trans_init,
        o3d.pipelines.registration.TransformationEstimationPointToPlane(),    # 执行点对面的ICP算法
        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=30))  # 设置最大迭代次数
print(icp_p2plane)  # 输出ICP相关信息
print("Transformation is:")
print(icp_p2plane.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_source1.pcd", source_temp)  # 保存点云
    o3d.visualization.draw_geometries([source_temp, target_temp], width=600, height=600, mesh_show_back_face=False)


draw_registration_result(source, target, icp_p2plane.transformation)

三、实现效果

3.1原始点云

3.2配准后点云

3.3计算数据

Initial alignment
RegistrationResult with fitness=1.000000e+00, inlier_rmse=1.838722e-01, and correspondence_set size of 327323
Access transformation to get result.
Apply point-to-plane ICP
RegistrationResult with fitness=1.000000e+00, inlier_rmse=1.660569e-07, and correspondence_set size of 327323
Access transformation to get result.
Transformation is:
[[ 1.00000001e+00  3.24158871e-10  5.54218013e-09 -3.07668001e-08]
 [ 1.41976912e-09  7.07106784e-01 -7.07106783e-01  4.99999999e+00]
 [-7.42239601e-10  7.07106783e-01  7.07106780e-01  1.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]