目錄

一、概述

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]]