1. 彩色点云配准的概念

本教程演示了一种结合几何和颜色进行配准的 ICP 变体，采用了 [Park2017] 的算法。颜色信息可以锁定切平面上的对齐，因此该算法比之前的点云配准算法更加精确且更具鲁棒性，同时运行速度与传统的 ICP 配准相当。本教程中使用了来自 ICP 配准的符号和表示方法。

在这里插入图片描述

2. 辅助可视化函数

为了展示彩色点云之间的对齐效果，draw_registration_result_original_color 函数用点云的原始颜色进行渲染。

def draw_registration_result_original_color(source, target, transformation):
    source_temp = copy.deepcopy(source)
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target],
                                      zoom=0.5,
                                      front=[-0.2458, -0.8088, 0.5342],
                                      lookat=[1.7745, 2.2305, 0.9787],
                                      up=[0.3109, -0.5878, -0.7468])

3. 处理输入数据

下面的代码从两个文件中读取源点云和目标点云。配准初始化时使用单位矩阵。

source = o3d.io.read_point_cloud("../../test_data/ColoredICP/frag_115.ply")
target = o3d.io.read_point_cloud("../../test_data/ColoredICP/frag_116.ply")

# 绘制初始对齐
current_transformation = np.identity(4)
draw_registration_result_original_color(source, target, current_transformation)

在这里插入图片描述

4. 点到平面 ICP

我们首先运行点到平面 ICP 作为基准方法。下方的可视化展示了未对齐的绿色三角形纹理，这是因为几何约束无法阻止两个平面表面滑动。

# point to plane ICP
current_transformation = np.identity(4)
print("2. Point-to-plane ICP registration is applied on original point")
print("   clouds to refine the alignment. Distance threshold 0.02.")
result_icp = o3d.pipelines.registration.registration_icp(
    source, target, 0.02, current_transformation,
    o3d.pipelines.registration.TransformationEstimationPointToPlane())
print(result_icp)
draw_registration_result_original_color(source, target,
                                        result_icp.transformation)

在这里插入图片描述

5. 彩色点云配准

彩色点云配准的核心函数是 registration_colored_icp。根据 [Park2017]，它运行 ICP 迭代（详情参见点到点 ICP），并使用联合优化目标函数：

$E(\mathbf{T}) = (1 - \delta) E_C(\mathbf{T}) + \delta E_G(\mathbf{T})$

其中， $\mathbf{T}$ 是要估计的变换矩阵。 $E_C$ 和 $E_G$ 分别是光度和几何项。 $\delta \in [0, 1]$ 是一个权重参数，通过经验确定。

几何项 $E_G$ 与点到平面 ICP 的目标相同：

$E_G(\mathbf{T}) = \sum_{(p,q) \in \mathcal{K}} \left( (\mathbf{p} - \mathbf{Tq}) \cdot \mathbf{n}_p \right)^2,$

其中， $\mathcal{K}$ 是当前迭代中的对应集， $\mathbf{n}_p$ 是点 $\mathbf{p}$ 的法线。

颜色项 $E_C$ 衡量点 $\mathbf{q}$ 的颜色（表示为 $C(\mathbf{q})$ ）与其在 $\mathbf{p}$ 切平面上的投影颜色之间的差异。

$E_C(\mathbf{T}) = \sum_{(p,q) \in \mathcal{K}} \left( C_p(\mathbf{f}(\mathbf{Tq})) - C(\mathbf{q}) \right)^2,$

其中， $C_p(\cdot)$ 是在 $\mathbf{p}$ 切平面上连续定义的预计算函数。函数 $\mathbf{f}(\cdot)$ 将 3D 点投影到切平面。更多详情请参考 [Park2017]。

为了进一步提高效率，[Park2017] 提出了多尺度配准方案，已在以下脚本中实现。

# colored pointcloud registration
# This is implementation of following paper
# J. Park, Q.-Y. Zhou, V. Koltun,
# Colored Point Cloud Registration Revisited, ICCV 2017
voxel_radius = [0.04, 0.02, 0.01]
max_iter = [50, 30, 14]
current_transformation = np.identity(4)
print("3. Colored point cloud registration")
for scale in range(3):
    iter = max_iter[scale]
    radius = voxel_radius[scale]
    print([iter, radius, scale])

    print("3-1. Downsample with a voxel size %.2f" % radius)
    source_down = source.voxel_down_sample(radius)
    target_down = target.voxel_down_sample(radius)

    print("3-2. Estimate normal.")
    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))

    print("3-3. Applying colored point cloud registration")
    result_icp = o3d.pipelines.registration.registration_colored_icp(
        source_down, target_down, radius, current_transformation,
        o3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-6,
                                                          relative_rmse=1e-6,
                                                          max_iteration=iter))
    current_transformation = result_icp.transformation
    print(result_icp)
draw_registration_result_original_color(source, target,
                                        result_icp.transformation)

在这里插入图片描述
总共创建了 3 层多分辨率点云，使用 voxel_down_sample 进行降采样。法线是通过顶点法线估计计算的。核心配准函数 registration_colored_icp 针对每一层从粗到细进行调用。lambda_geometric 是 registration_colored_icp 的一个可选参数，用于确定整体能量中的 (\lambda \in [0, 1])，即 (\lambda E_G + (1 - \lambda) E_C)。

输出是两个点云的紧密对齐。请注意墙上的绿色三角形。