鲁棒核函数

译注

本篇译自 Open3D 官方教程 Robust kernels（0.19.0）。代码、输出、API 名称保持原文未作改动。

本教程演示如何在离群点剔除（outlier rejection）场景中使用鲁棒核函数（robust kernels）。在本教程中，我们以 ICP（Iterative Closest Point，迭代最近点）配准算法作为目标问题来处理离群点。尽管如此，这里的理论适用于任意给定的优化问题，而不仅限于 ICP。目前，鲁棒核函数仅针对 PointToPlane ICP 实现了支持。

Open3D 中采用的记号以及部分核函数的实现，灵感来自论文《Analysis of Robust Functions for Registration Algorithms》[Babin2019]。

说明

本教程以及 Open3D 中鲁棒核函数的原始实现，由波恩大学的 Ignacio Vizzo 和 Cyrill Stachniss 贡献。

输入数据

下面的代码从两个文件中分别读取源点云和目标点云，并给出一个粗略的变换矩阵。

说明

初始对齐通常由全局配准（global registration）算法得到。相关示例请参见全局配准。

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

demo_icp_pcds = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])
target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])
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)

输出：

[Open3D WARNING] GLFW Error: Failed to detect any supported platform
[Open3D WARNING] GLFW initialized for headless rendering.

使用鲁棒核的 Point-to-plane ICP

标准的 point-to-plane ICP 算法 [ChenAndMedioni1992] 最小化如下目标函数：

$$E(\mathbf{T}) = \sum_{(\mathbf{p},\mathbf{q})\in\mathcal{K}}\big((\mathbf{p} - \mathbf{T}\mathbf{q})\cdot\mathbf{n}_{\mathbf{p}}\big)^{2},$$

其中 $\mathbf{n}_{\mathbf{p}}$ 是点 $\mathbf{p}$ 的法向量，$\mathcal{K}$ 是目标点云 $\mathbf{P}$ 与源点云 $\mathbf{Q}$ 之间的对应关系集合。

若记 $r_i(\mathbf{T})$ 为第 $i$ 个残差，对于给定的一对对应关系 $(\mathbf{p},\mathbf{q})\in\mathcal{K}$，可将目标函数改写为：

$$E(\mathbf{T}) = \sum_{(\mathbf{p},\mathbf{q})\in\mathcal{K}}\big((\mathbf{p} - \mathbf{T}\mathbf{q})\cdot\mathbf{n}_{\mathbf{p}}\big)^{2} = \sum_{i=1}^{N} \big({r_i(\mathbf{T})}\big)^2$$

上述优化问题也可以采用迭代重加权最小二乘（Iteratively Reweighted Least-Squares, IRLS）方法求解，它通过求解一系列加权最小二乘问题来实现：

$$E(\mathbf{T}) = \sum_{i=1}^{N} w_i \big({r_i(\mathbf{T})}\big)^2$$

基于鲁棒核的离群点剔除

鲁棒损失（robust loss）的核心思想是：对那些被认为由离群点产生的大残差进行降权，从而削弱它们对解的影响。这通过对 $E(\mathbf{T})$ 进行如下优化来实现：

$$E(\mathbf{T}) = \sum_{(\mathbf{p},\mathbf{q})\in\mathcal{K}}\rho\big((\mathbf{p} - \mathbf{T}\mathbf{q})\cdot\mathbf{n}_{\mathbf{p}}\big) = \sum_{i=1}^{N} \rho\big({r_i(\mathbf{T})}\big),$$

其中 $\rho(r)$ 也被称为鲁棒损失函数或核函数（kernel）。

可以看到，IRLS 中的优化形式与采用鲁棒损失函数的形式之间存在某种联系。通过令权重 $w_i= \frac{1}{r_i(\mathbf{T})}\rho'(r_i(\mathbf{T}))$，就可以利用现有的加权最小二乘技术来求解鲁棒损失优化问题。因此，我们可以使用 Gauss-Newton 法最小化目标函数，并通过迭代求解下式来确定增量：

$$\left(\mathbf{J}^\top \mathbf{W} \mathbf{J}\right)^{-1}\mathbf{J}^\top\mathbf{W}\vec{r},$$

其中 $\mathbf{W} \in \mathbb{R}^{N\times N}$ 是一个对角矩阵，对角元素为每个残差 $r_i$ 对应的权重 $w_i$。

如何在 Open3D 中使用鲁棒核

registration_icp 可以带一个参数 TransformationEstimationPointToPlane(loss) 来调用。其中 loss 是一个给定的损失函数（也称作鲁棒核）。

在内部，TransormationEstimationPointToPlane(loss) 实现了一个函数，用于根据所提供的鲁棒核，计算 point-to-plane ICP 目标的加权残差和雅可比矩阵。

普通 ICP 与鲁棒 ICP 的对比

为了更好地展示在配准中使用鲁棒核的优势，我们人为地向源点云添加一些高斯噪声。

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


mu, sigma = 0, 0.1  # mean and standard deviation
source_noisy = apply_noise(source, mu, sigma)

print("Source PointCloud + noise:")
o3d.visualization.draw_geometries([source_noisy],
                                  zoom=0.4459,
                                  front=[0.353, -0.469, -0.809],
                                  lookat=[2.343, 2.217, 1.809],
                                  up=[-0.097, -0.879, 0.467])

输出：

Source PointCloud + noise:
[Open3D WARNING] GLFW initialized for headless rendering.

普通 ICP

我们来看看，如果直接沿用 ICP 配准教程中使用过的完全相同参数，结果会怎样。

threshold = 0.02
print("Vanilla point-to-plane ICP, threshold {}: {}".format(threshold))
p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane()
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)

输出：

Vanilla point-to-plane ICP,
RegistrationResult with fitness 0.1xxxx and correspondence_set size of 19921
Access transformation to get result.
Transformation is:
[[ 0.8566005   0.01966479 -0.51582045  0.50097926]
 [-0.15647173  0.96255541 -0.22285079  0.78844504]
 [ 0.4912089   0.27080504  0.82750577 -1.43586132]
 [ 0.          0.          0.          1.        ]]
[Open3D WARNING] GLFW initialized for headless rendering.

调参普通 ICP

既然我们现在面对的是高斯噪声，我们或许可以尝试增大阈值来搜索最近邻，以期改善配准结果。

可以看到，在这样的条件下且不使用鲁棒核时，传统 ICP 完全无法处理离群点。

threshold = 1.0
print("Vanilla point-to-plane ICP, threshold {}: {}".format(threshold))
p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane()
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)

输出：

Vanilla point-to-plane ICP,
RegistrationResult with fitness 0.1xxxx and correspondence_set size of 198835
Access transformation to get result.
Transformation is:
[[ 0.79176699  0.06858295 -0.60724278  1.53567654]
 [-0.29950932  0.91009785 -0.28778649  1.26588549]
 [ 0.53191466  0.40896612  0.7409009  -1.47497048]
 [ 0.          0.          0.          1.        ]]
[Open3D WARNING] GLFW initialized for headless rendering.

鲁棒 ICP

使用相同的 threshold=1.0，并配合一个鲁棒核，就能正确地把两个点云配准起来：

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)

输出：

Robust point-to-plane ICP,
Using robust loss: RobustKernel::TukeyLoss with k=0.100000
RegistrationResult with fitness 0.1xxxx and correspondence_set size of 186041
Access transformation to get result.
Transformation is:
[[ 0.83218598  0.04077245 -0.55323816  0.59712462]
 [-0.18678854  0.96004183 -0.21000923  0.90890328]
 [ 0.52164216  0.27729459  0.80642585 -1.51385962]
 [ 0.          0.          0.          1.        ]]
[Open3D WARNING] GLFW initialized for headless rendering.

说明

本例使用的是 open3d.pipelines.registration.TukeyLoss 中提供的 TukeyLoss。对于参数 k，我们将其设为与噪声模型的标准差一致（$k = \sigma$）。鲁棒核中的参数 k 通常选取为与输入数据噪声模型的标准差相匹配的值。从这个意义上说，k 是区分内点（inlier）与离群点（outlier）的判据。虽然在真实数据中这一点并不总是容易确定，但对于合成数据而言，为了直观展示鲁棒核的优越性，可以很容易地固定该值。