之前我們有介紹過(guò)ICP配準(zhǔn)算法的基本原理：ICP配準(zhǔn)的基本原理
如果我們?cè)趯W(xué)習(xí)的時(shí)候手頭沒(méi)有點(diǎn)云數(shù)據(jù)，我們可以使用官方給出的Demo：官方Demo。在這個(gè)庫(kù)中，提供了ICP迭代最近點(diǎn)算法及其變體秧荆。本文主要記錄了次庫(kù)API的使用方法疯特。

1 點(diǎn)到點(diǎn)的ICP

1.1 輔助可視化函數(shù)

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],
                                      zoom=0.4459,
                                      front=[0.9288, -0.2951, -0.2242],
                                      lookat=[1.6784, 2.0612, 1.4451],
                                      up=[-0.3402, -0.9189, -0.1996])

source = o3d.io.read_point_cloud("./data/cloud_bin_0.pcd")
target = o3d.io.read_point_cloud("./data/cloud_bin_1.pcd")
threshold = 0.02
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)

1.2 點(diǎn)到點(diǎn)ICP API

print("Apply point-to-point ICP")
reg_p2p = o3d.pipelines.registration.registration_icp(
    source, target, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint(),
    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=4000))
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)

在這里淹办，我們可以調(diào)整一些參數(shù)來(lái)看看此算法的性能印荔，和每個(gè)參數(shù)的具體意義攻询。
reg_p2p = o3d.pipelines.registration.registration_icp(source, target, threshold, trans_init, o3d.pipelines.registration.TransformationEstimationPointToPoint(), o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=4000)) 這是調(diào)用 ICP 配準(zhǔn)的主要函數(shù)从撼，它返回一個(gè) RegistrationResult 對(duì)象，包含了配準(zhǔn)的結(jié)果和評(píng)估指標(biāo)钧栖。它的參數(shù)分別是：

• source 是源點(diǎn)云低零，一個(gè) open3d.geometry.PointCloud 對(duì)象，表示要被變換的點(diǎn)云拯杠。
• target 是目標(biāo)點(diǎn)云掏婶，一個(gè) open3d.geometry.PointCloud 對(duì)象，表示要與源點(diǎn)云對(duì)齊的點(diǎn)云潭陪。
• threshold 是最大對(duì)應(yīng)點(diǎn)對(duì)距離雄妥，一個(gè)浮點(diǎn)數(shù)，表示兩個(gè)點(diǎn)云中的點(diǎn)可以被認(rèn)為是對(duì)應(yīng)的最大距離依溯。超過(guò)這個(gè)距離的點(diǎn)對(duì)會(huì)被忽略茎芭。
• trans_init 是初始變換矩陣，一個(gè) 4x4 的 numpy.ndarray 數(shù)組誓沸，表示在開(kāi)始配準(zhǔn)之前，對(duì)源點(diǎn)云進(jìn)行的預(yù)變換壹粟。如果沒(méi)有預(yù)變換拜隧，可以使用單位矩陣。

o3d.pipelines.registration.TransformationEstimationPointToPoint()是變換估計(jì)方法趁仙，一個(gè) open3d.pipelines.registration.TransformationEstimation 對(duì)象洪添，表示使用點(diǎn)到點(diǎn)的方式計(jì)算變換矩陣。這種方式假設(shè)源點(diǎn)云和目標(biāo)點(diǎn)云的點(diǎn)是一一對(duì)應(yīng)的雀费，然后使用最小二乘法求解變換矩陣干奢。這是最簡(jiǎn)單的一種變換估計(jì)方法，也可以使用其他的方法盏袄，如點(diǎn)到平面忿峻、廣義 ICP、彩色 ICP 等辕羽。

• open3d.pipelines.registration.TransformationEstimationPointToPoint
• open3d.pipelines.registration.TransformationEstimationPointToPlane
• open3d.pipelines.registration.TransformationEstimationForColoredICP

o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=4000)是收斂準(zhǔn)則逛尚，一個(gè) open3d.pipelines.registration.ICPConvergenceCriteria 對(duì)象，表示配準(zhǔn)過(guò)程的終止條件刁愿。它有三個(gè)參數(shù)：relative_fitness绰寞、relative_rmse 和 max_iteration，分別表示相對(duì)適應(yīng)度、相對(duì)均方根誤差和最大迭代次數(shù)滤钱。當(dāng)任意一個(gè)條件滿足時(shí)觉壶，配準(zhǔn)過(guò)程就會(huì)停止。默認(rèn)的值分別是 1e-6件缸、1e-6 和 30铜靶。

（1）最大對(duì)應(yīng)點(diǎn)的距離的設(shè)置方法（即，代碼中的threshold）：

# For Vanilla ICP (double)
max_correspondence_distance = 0.07

# For Multi-Scale ICP (o3d.utility.DoubleVector):
max_correspondence_distances = o3d.utility.DoubleVector([0.3, 0.14, 0.07])

（2）初始變換矩陣的初始化：
初始對(duì)齊通常通過(guò)全局配準(zhǔn)算法獲得停团。

# Initial alignment or source to target transform.
init_source_to_target = 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]])

（3）估計(jì)方法
這將設(shè)置ICP方法來(lái)計(jì)算給定對(duì)應(yīng)關(guān)系的兩個(gè)點(diǎn)云之間的轉(zhuǎn)換旷坦。這里就可以設(shè)置是使用點(diǎn)到點(diǎn)的ICP還是使用點(diǎn)到面等方法。這里還可以設(shè)置不同的核佑稠，例如：robust kernels（用于去除離群點(diǎn)）

# Select the `Estimation Method`, and `Robust Kernel` (for outlier-rejection).
estimation = treg.TransformationEstimationPointToPlane()

# 魯棒核
robust_kernel = o3d.t.pipelines.registration.robust_kernel.RobustKernel(method, scale, shape)

核的選擇還有這些選項(xiàng)：

• robust_kernel.RobustKernelMethod.L2Loss
• robust_kernel.RobustKernelMethod.L1Loss
• robust_kernel.RobustKernelMethod.HuberLoss
• robust_kernel.RobustKernelMethod.CauchyLoss
• robust_kernel.RobustKernelMethod.GMLoss
• robust_kernel.RobustKernelMethod.TukeyLoss
• robust_kernel.RobustKernelMethod.GeneralizedLoss
  代碼示例：

print("Robust point-to-plane ICP, threshold={}:".format(threshold))
loss = o3d.pipelines.registration.TukeyLoss(k=sigma)
print("Using robust loss:", loss)
p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane(loss)
reg_p2l = o3d.pipelines.registration.registration_icp(source_noisy, target,
                                                      threshold, trans_init,
                                                      p2l)
print(reg_p2l)
print("Transformation is:")
print(reg_p2l.transformation)
draw_registration_result(source, target, reg_p2l.transformation)

（4）ICP收斂標(biāo)準(zhǔn)
這里是設(shè)置ICP算法的終止條件秒梅。

# Convergence-Criteria for Vanilla ICP:

criteria = treg.ICPConvergenceCriteria(relative_fitness=0.000001, relative_rmse=0.000001, max_iteration=50)

# List of Convergence-Criteria for Multi-Scale ICP:

criteria_list = [
    treg.ICPConvergenceCriteria(relative_fitness=0.0001, relative_rmse=0.0001, max_iteration=20),
    treg.ICPConvergenceCriteria(0.00001, 0.00001, 15),
    treg.ICPConvergenceCriteria(0.000001, 0.000001, 10)
]

（5）體素大小
體素大小（較低的體素大小對(duì)應(yīng)于較高的分辨率）舌胶，適用于多尺度ICP的每個(gè)尺度捆蜀。我們希望在粗點(diǎn)云（低分辨率或大體素大小）上執(zhí)行初試迭代（因?yàn)樗r(shí)幔嫂，并且避免局部最小值）辆它，然后移動(dòng)到密集的點(diǎn)云（高分辨率或小體素大小）履恩。因此锰茉，體素大小必須嚴(yán)格降序排列。

# Vanilla ICP
voxel_size = 0.025

# Lower `voxel_size` is equivalent to higher resolution,
# and we want to perform iterations from coarse to dense resolution,
# therefore `voxel_sizes` must be in strictly decressing order.
voxel_sizes = o3d.utility.DoubleVector([0.1, 0.05, 0.025])

2 多尺度ICP示例

通過(guò)上面的學(xué)習(xí)切心，我們應(yīng)該大概知道了ICP的使用方法飒筑，我們不難發(fā)現(xiàn)初始化的對(duì)齊對(duì)算法的影響極大，不良的初始化直接會(huì)導(dǎo)致ICP收斂失敗绽昏。那如何去解決這個(gè)問(wèn)題呢协屡？通常會(huì)有兩個(gè)方案：

• 增大迭代次數(shù)，這樣是有可能會(huì)解決此問(wèn)題全谤，但是這往往需要非常長(zhǎng)的時(shí)間
• 使用多尺度ICP肤晓，在不同尺度上完成粗配準(zhǔn)和精配準(zhǔn)。

使用Vanilla-ICP時(shí)出現(xiàn)的問(wèn)題：

• 在密集的點(diǎn)云上運(yùn)ICP算法非常慢认然。
• 它需要良好的初始對(duì)齊方式：

如果點(diǎn)云沒(méi)有很好地對(duì)齊补憾，收斂可能會(huì)在初始迭代中卡在局部最小值中。如果對(duì)齊的點(diǎn)云沒(méi)有足夠的重疊卷员，我們需要有一個(gè)更大的max_correspondence_distance余蟹。如果點(diǎn)云被大量下采樣（粗），則獲得的結(jié)果將不準(zhǔn)確子刮。

這些缺點(diǎn)可以使用多尺度ICP解決威酒。在多尺度ICP中窑睁，我們?cè)诖贮c(diǎn)云上執(zhí)行初試迭代，以獲得對(duì)初始對(duì)齊的更好估計(jì)葵孤，并使用此對(duì)齊方式在更密集的點(diǎn)云上收斂担钮。粗點(diǎn)云上的ICP非常昂貴，并且允許我們使用更大的max_correspondence_distance尤仍。收斂也不太可能卡在局部最小值中箫津。當(dāng)我們得到一個(gè)很好的估計(jì)值時(shí)，在密集的點(diǎn)云上需要更少的迭代來(lái)收斂到更準(zhǔn)確的變換宰啦。

所以苏遥，建議使用Multi-Scale ICP替代ICP，以實(shí)現(xiàn)高效收斂赡模，特別是對(duì)于大型點(diǎn)云田炭。

import open3d as o3d
import numpy as np 
import copy 
import time 
import open3d.t.pipelines.registration as treg

def draw_registration_result(source, target, transformation):
    source_temp = source.clone()
    target_temp = target.clone()

    source_temp.transform(transformation)

    # This is patched version for tutorial rendering.
    # Use `draw` function for you application.
    o3d.visualization.draw_geometries(
        [source_temp.to_legacy(),
         target_temp.to_legacy()],
        zoom=0.4459,
        front=[0.9288, -0.2951, -0.2242],
        lookat=[1.6784, 2.0612, 1.4451],
        up=[-0.3402, -0.9189, -0.1996])
    
def apply_noise(pcd, mu, sigma):
    noisy_pcd = copy.deepcopy(pcd)
    points = np.asarray(noisy_pcd.points)
    points += np.random.normal(mu, sigma, size=points.shape)
    noisy_pcd.points = o3d.utility.Vector3dVector(points)
    return noisy_pcd

    
if __name__ =="__main__":
    demo_icp_pcds = o3d.data.DemoICPPointClouds()
    source = o3d.t.io.read_point_cloud(demo_icp_pcds.paths[0])
    target = o3d.t.io.read_point_cloud(demo_icp_pcds.paths[1])

    # source = o3d.t.io.read_point_cloud("./data/cloud_bin_0.pcd")
    # target = o3d.t.io.read_point_cloud("./data/cloud_bin_1.pcd")
        
    voxel_sizes = o3d.utility.DoubleVector([0.1, 0.05, 0.025])

    # List of Convergence-Criteria for Multi-Scale ICP:
    criteria_list = [
        treg.ICPConvergenceCriteria(relative_fitness=0.0001,
                                    relative_rmse=0.0001,
                                    max_iteration=20),
        treg.ICPConvergenceCriteria(0.00001, 0.00001, 15),
        treg.ICPConvergenceCriteria(0.000001, 0.000001, 10)
    ]

    # `max_correspondence_distances` for Multi-Scale ICP (o3d.utility.DoubleVector):
    max_correspondence_distances = o3d.utility.DoubleVector([0.3, 0.02, 0.001])

    # Initial alignment or source to target transform.
    init_source_to_target = o3d.core.Tensor.eye(4, o3d.core.Dtype.Float32)

    # Select the `Estimation Method`, and `Robust Kernel` (for outlier-rejection).
    estimation = treg.TransformationEstimationPointToPlane()

    # Save iteration wise `fitness`, `inlier_rmse`, etc. to analyse and tune result.
    callback_after_iteration = lambda loss_log_map : print("Iteration Index: {}, Scale Index: {}, Scale Iteration Index: {}, Fitness: {}, Inlier RMSE: {},".format(
        loss_log_map["iteration_index"].item(),
        loss_log_map["scale_index"].item(),
        loss_log_map["scale_iteration_index"].item(),
        loss_log_map["fitness"].item(),
        loss_log_map["inlier_rmse"].item()))
    
    s = time.time()

    registration_ms_icp = treg.multi_scale_icp(source, target, voxel_sizes,
                                            criteria_list,
                                            max_correspondence_distances,
                                            init_source_to_target, estimation,
                                            callback_after_iteration)

    ms_icp_time = time.time() - s
    print("Time taken by Multi-Scale ICP: ", ms_icp_time)
    print("Inlier Fitness: ", registration_ms_icp.fitness)
    print("Inlier RMSE: ", registration_ms_icp.inlier_rmse)

    draw_registration_result(source, target, registration_ms_icp.transformation)

參考博客：
【1】官方文檔
【2】https://jiatianzhi.cn/index.php/archives/84/
【3】https://jiatianzhi.cn/index.php/archives/93/