ICP配准代码的实现（python+open3D）

作者: 小黄不头秃 | 来源:发表于2024-03-05 15:45 被阅读0次

之前我们有介绍过ICP配准算法的基本原理：ICP配准的基本原理
如果我们在学习的时候手头没有点云数据，我们可以使用官方给出的Demo：官方Demo。在这个库中，提供了ICP迭代最近点算法及其变体。本文主要记录了次库API的使用方法。

1 点到点的ICP

1.1 辅助可视化函数

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 点到点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)

在这里，我们可以调整一些参数来看看此算法的性能，和每个参数的具体意义。
reg_p2p = o3d.pipelines.registration.registration_icp(source, target, threshold, trans_init, o3d.pipelines.registration.TransformationEstimationPointToPoint(), o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=4000)) 这是调用 ICP 配准的主要函数，它返回一个 RegistrationResult 对象，包含了配准的结果和评估指标。它的参数分别是：

source 是源点云，一个 open3d.geometry.PointCloud 对象，表示要被变换的点云。
target 是目标点云，一个 open3d.geometry.PointCloud 对象，表示要与源点云对齐的点云。
threshold 是最大对应点对距离，一个浮点数，表示两个点云中的点可以被认为是对应的最大距离。超过这个距离的点对会被忽略。
trans_init 是初始变换矩阵，一个 4x4 的 numpy.ndarray 数组，表示在开始配准之前，对源点云进行的预变换。如果没有预变换，可以使用单位矩阵。

o3d.pipelines.registration.TransformationEstimationPointToPoint()是变换估计方法，一个 open3d.pipelines.registration.TransformationEstimation 对象，表示使用点到点的方式计算变换矩阵。这种方式假设源点云和目标点云的点是一一对应的，然后使用最小二乘法求解变换矩阵。这是最简单的一种变换估计方法，也可以使用其他的方法，如点到平面、广义 ICP、彩色 ICP 等。

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

o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=4000)是收敛准则，一个 open3d.pipelines.registration.ICPConvergenceCriteria 对象，表示配准过程的终止条件。它有三个参数：relative_fitness、relative_rmse 和 max_iteration，分别表示相对适应度、相对均方根误差和最大迭代次数。当任意一个条件满足时，配准过程就会停止。默认的值分别是 1e-6、1e-6 和 30。

（1）最大对应点的距离的设置方法（即，代码中的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）初始变换矩阵的初始化：
初始对齐通常通过全局配准算法获得。

# 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）估计方法
这将设置ICP方法来计算给定对应关系的两个点云之间的转换。这里就可以设置是使用点到点的ICP还是使用点到面等方法。这里还可以设置不同的核，例如：robust kernels（用于去除离群点）

# 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)

核的选择还有这些选项：

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收敛标准
这里是设置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）体素大小
体素大小（较低的体素大小对应于较高的分辨率），适用于多尺度ICP的每个尺度。我们希望在粗点云（低分辨率或大体素大小）上执行初试迭代（因为它更省时，并且避免局部最小值），然后移动到密集的点云（高分辨率或小体素大小）。因此，体素大小必须严格降序排列。

# 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示例

通过上面的学习，我们应该大概知道了ICP的使用方法，我们不难发现初始化的对齐对算法的影响极大，不良的初始化直接会导致ICP收敛失败。那如何去解决这个问题呢？通常会有两个方案：

增大迭代次数，这样是有可能会解决此问题，但是这往往需要非常长的时间
使用多尺度ICP，在不同尺度上完成粗配准和精配准。

使用Vanilla-ICP时出现的问题：

在密集的点云上运ICP算法非常慢。
它需要良好的初始对齐方式：

如果点云没有很好地对齐，收敛可能会在初始迭代中卡在局部最小值中。如果对齐的点云没有足够的重叠，我们需要有一个更大的max_correspondence_distance。如果点云被大量下采样（粗），则获得的结果将不准确。

这些缺点可以使用多尺度ICP解决。在多尺度ICP中，我们在粗点云上执行初试迭代，以获得对初始对齐的更好估计，并使用此对齐方式在更密集的点云上收敛。粗点云上的ICP非常昂贵，并且允许我们使用更大的max_correspondence_distance。收敛也不太可能卡在局部最小值中。当我们得到一个很好的估计值时，在密集的点云上需要更少的迭代来收敛到更准确的变换。

所以，建议使用Multi-Scale ICP替代ICP，以实现高效收敛，特别是对于大型点云。

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/

ICP配准代码的实现（python+open3D）

1 点到点的ICP

1.1 辅助可视化函数

1.2 点到点ICP API

2 多尺度ICP示例

相关文章

网友评论

延伸阅读

深度阅读

栏目导航

热点阅读