结合方向包围框改进的三维点云配准算法

兰文昊，李 宁，佟 强

1.北京信息科技大学 自动化学院，北京 100101

2.北京信息科技大学 计算机学院，北京 100101

近年来，随着深度传感器成本的降低，能够直接获取三维数据的设备越来越普及。三维点云作为三维数据简单且有效的表现形式，广泛应用于机器人、自动驾驶和虚拟/增强现实应用中。三维点云配准算法通过估算待配准点云间的对应关系，获取待配准点云间的变换矩阵，已成为近几年领域内的研究热点[1-4]。这些研究主要聚焦在三维点云配准的精度、时间效率和鲁棒性等方面，提出了许多不同的方法。

Besl等人提出最近点迭代配准算法ICP，使用最小二乘法和奇异值分解来估算待配准两团点云间的变换关系，并用取得的变换参数更新点云间的对应关系，迭代上述步骤直到完成三维点云配准[5]。ICP因为其简单易用，成为三维点云配准领域的经典算法，经常被用于精细配准阶段。在此后的研究中，Segal等人融合ICP和point-to-plane ICP，提出Generalized-ICP，改善了点云配准算法对噪声和遮挡的鲁棒性[6]；Bouaziz等人使用稀疏诱导范数重新定义ICP，提升了ICP对于外点和不完整点云数据的鲁棒性[7]；Biber等人使用正态分布变换和牛顿法简化了点云间对应关系的探寻过程[8]；Agamennoni等人在全概率模型上应用统计推理技术，提出一种新的概率数据关联策略，提升了配准算法对外点和噪声的鲁棒性[9]；Yang等人提出TEASER方法，使用截断最小二乘和半定松弛优化，剔除了外点的干扰[10]。

然而，ICP及其改进算法的解空间中，至少存在两个不同的解，使得目标函数取得局部最优值，即非凸问题。由于非凸问题，配准结果的精度高度依赖源点云和模板点云的初始相对位置。如果待配准点云间的初始相对偏转角度过大，ICP就会陷入局部最优，损失配准精度。为了解决非凸问题，找到全局最优解，Yang等人提出Go-ICP，使用分支定界的方法搜索三维变换空间SE(3)[11]。虽然Go-ICP能够得到全局最优解，但是算法的时间复杂度是ICP及其改进算法的数倍，不能满足实时性需要。在其他关于点云配准全局最优解的研究中，还包括混合整数规划、黎曼优化和凸松弛[12-14]。

传统三维点云配准方法除了非凸问题，计算效率也是一大障碍。传统方法由于需要明确估计点云间的对应关系，计算效率会随着待配准点云中点数的增加而降低。随着深度学习技术在三维点云上的发展，传统方法中的问题，有了新的解决思路[15-17]。

Aoki等人将深度学习引入三维点云配准领域，提出PointNetLK[18]，使用深度神经网络PointNet[19]和修改过的LK[20]算法来完成三维点云配准任务，相对于传统算法，提升了配准效率。PCRNet[21]受PointNetLK的启发，使用PointNet提取三维点云的特征，并采用孪生架构的神经网络来级联点云间的特征向量，使用学习的方法非迭代地获得点云变换参数，一定程度上改善了算法对于噪声的抗干扰能力。

还有一部分研究尝试设计局部描述符来寻找源点云和目标点云间的对应关系，如尺度不变曲率描述符[22]、FPFH[23]。由于手工设计特征的局限性，一些学者开始研究用深度学习来提取三维局部描述符。3DMatch[24]从RGBD三维重建数据中自监督学习局部特征描述符。PPFNet[25]结合点云局部特征和整体特征，训练出具有全局信息的局部描述符。PPF-FoldNet[26]使用无监督学习，得到具有旋转不变性局部描述符。但是这些方法，只能适用于特定场景。

基于深度学习的PointNetLK使用PointNet对点云进行降维，使算法的时间复杂度不再随着点数的增加而增高。同时，能够推广至更广泛的应用场景中。PointNetLK把PointNet看作三维点云的“成像”函数，并使用二维光度追踪算法LK来对齐得到的“像”，虽然避免了传统三维点云配准算法的非凸问题。但是，由于它所使用的PointNet缺乏卷积结构，所提取到的点云特征缺乏点云表面的局部信息，又会陷入另一种形式的非凸问题，其直接结果是，当两个点云的偏差角度过大时，会影响配准的精度。

本文试图针对这个问题，对PointNetLK算法进行改进。

1 原始PointNetLK算法

PointNetLK为了降低点云配准的计算成本，使用深度学习技术提取点云特征，达到对点云降维的目的。这个过程可以理解为对点云进行“成像”。得到源点云和目标点云的“像”后，PointNetLK使用二维光流追踪算法LK，去获得两“像”之间的变换参数，从而导出三维点云间的转动参数，进而完成配准。

但是，PointNetLK中用于提取点云特征的深度网络PointNet缺乏卷积结构，仅能得到带有坐标信息的点云的宏观特征，导致PointNet成为一个多峰“成像”函数。其结果就是，PointNetLK陷入非凸优化问题。

下面用大写粗斜体字母表示矩阵和三维点集，如P，小写粗斜体表示列向量和N维点，如t，大写斜体字母表示常量，如M，小写斜体表示标量，如j。

1.1 三维点云配准的一般目标

其中L=min{M,N}。

旋转矩阵R和平移向量t通常表示为齐次形式，如式（2）所示：

三维点云配准的预期效果如图1所示。传统配准算法，需要点云中所有点都参与到配准过程中，这使得算法运行时间会随着点云规模扩大而增加。借助于深度学习技术，可以从点云中提取适合于配准目标的特征，且特征的维度远小于原始点云的规模，可望降低计算成本，并解决传统迭代配准算法中的多极值优化，即非凸问题。

图1 飞机点云的配准过程Fig.1 Registration process of aircraft point cloud

1.2 PointNetLK中点云特征的提取

如1.1节所述，用矩阵形式表示三维点云P和Q，即P∈ℝ3×N和Q∈ℝ3×M。

把PointNet看作是一个三维点云的“成像”函数Ψ:ℝ3×N→ℝK。函数Ψ把一个多层感知机（MLP）应用到三维点云中的每个点上，使N个三维点转变为N个K维点，即ℝ3×N→ℝK×N,接着进行最大池化操作从N个K维点中，选取一个最显著的K维点作为三维点云的K维全局特征向量，即ℝK×N→ℝK，即三维点云的“像”。

Ψ虽然起到了对三维点云降维的作用，达到了降低计算量的目的，但是，其本身是由MLP构成的深度学习网络，缺乏卷积结构，不具备提取点云表面局部微观特征的能力。这将导致Ψ(P)和Ψ(Q)仅能反映点云P和Q的宏观轮廓特征。LK算法无法捕获点云P和Q表面细节的坐标信息差异。其结果就是，配准易陷入非凸问题，停滞在局部最优状态。

1.3 光流追踪与点云变换的纽带-指数映射

1.1节使用旋转矩阵R和平移向量t表示三维空间中的坐标变换T。为了方便使用LK[20]算法获取坐标变换矩阵的更新参数，这里采用指数映射形式来表示三维空间中的坐标变换，空间坐标变换矩阵T表示如下：

其中：

为指数映射的产生算子，与转动参数δi一一对应。如果把P看成是模板点云，把Q看成是源点云，则三维点云配准的目标可以转化为找到最佳三维空间坐标变换T，使得：

其中，·表示对源点云Q进行三维空间中的坐标变换。

1.4 配准目标与光流追踪的结合

由于配准过程中，源点云的坐标是动态变化的，借鉴反向组合LK算法，实现只计算一次模板点云P对于无穷小转动参数δ的三维空间坐标变化率就能够完成算法迭代过程，从而减小计算量。根据上述思想，把三维点云配准的目标的形式变为：

式（5）等号右边可以展开为：

为了方便计算，采用有限差分梯度去近似Ψ对于转动参数δ的梯度。Jacobian矩阵用J表示，则J的第i列Ji的计算过程为：

其中，δi取非常小的固定值来代替无穷小量。

转动参数δ可以由下面公式计算出：

其中，J+是J的广义逆。

在迭代配准过程中，每次迭代都会用转动参数δ更新源点云Q：

PointNetLK阶段迭代结束后，源点云的三维空间坐标变换矩阵可以表示为：

PointNetLK的整体流程如图2所示，其中P为模板点云，Q为源点云，虚线框内为算法循环部分。

虽然PointNetLK跳出了传统迭代配准的非凸问题，但由于缺乏卷积结构的PointNet仅能提取到含有坐标信息的点云的宏观特征，导致点云偏转角度过大时，配准算法又陷入新型局部最优问题，直观配准结果如图3所示。

把PointNet看成是转动参数δ的函数Ψ(δ)，在源点云和目标点云初始偏转角度过大的情况下，使用LK算法迭代更新δ时，会存在至少两个不同的转动参数δ1和δ2,Ψ(δ1)=Ψ(δ2)。这时，式（8）等号右侧为0，δ停止更新，配准算法陷入局部最优，损失配准精度，如图4所示。

图2 PointNetLK算法流程图Fig.2 Flow chart of PointNetLK algorithm

图3 PointNetLK阶段对偏转角度较大点云配准结果Fig.3 Registration results of point clouds with larger deflection angle in PointNetLK phase

图4 PointNetLK陷入局部最优Fig.4 PointNetLK falling into local optimum

2 对PointNetLK算法的改进

如图4分析，PointNetLK在源点云和目标点云初始相对偏转角度过大的情况下，陷入局部最优。本文针对这个问题，提出了PointNetLK-OBB算法。该算法在PointNetLK的基础上，利用能够刻画点云全局规整特征的三维几何工具Oriented Bounding Box[27-28]，在ICP的指引下，避开PointNetLK语境下的非凸问题，从而提升偏转角度过大的点云间的配准精度。

2.1 Oriented Bounding Box的构造

Oriented Bounding Box（OBB）是指能够包围三维点云的最小体积，通常由一个中心点，三个表示方向的正交单位向量和三个表示长宽高延展半径的标量{l,w,h}来表示。

三维点云的Axis Aligned Bounding Box（AABB）和OBB一样，都是三维空间中的几何描述工具。考虑一般点云X∈ℝ3×N的OBB和AABB，分别记为OBBX和AABBX。OBBX可以看成点云X经过某一空间旋转R∈ℝ3×3后得到的X′的轴对齐包围框AABBX′，即：

所以Oriented Bounding Box的求解问题转变为，从特殊正交群里找到一个合适的三维旋转R∈SO(3,ℝ)，使得AABBX′的体积最小：

OBB的求解使用Hybrid Bounding Box Rotation Identification[28]。

2.2 三维点云配准目标与Oriented Bounding Box的结合

式（10）给出了PointNetLK阶段获取到的能够对齐Q和P主体特征的空间变换TPointNetLK。用TPointNetLK更新后的源点云记为Q′：

分别记Q′和P的OBB为OBBQ′和OBBP，为了利用OBB的规整性完成三维点云配准任务，需要将OBBQ′和OBBP对齐。为了对齐OBBQ′和OBBP，使用ICP对齐OBBQ′和OBBP的顶点。记OBBQ′的顶点为ℝ3×8，OBBP的顶点为POBB∈ℝ3×8。

一般点云X的方向包围框OBBX的顶点可以看作式（10）中AABBX′的顶点经历了一个空间旋转R-1，所以OBBX的八个顶点为：

因为点云的OBB刻画点云的主体特征，所以应用在OBB的八个顶点上的空间变换，同样可以应用在点云中的每个点上。在式（13）的基础上，继续对PointNetLK阶段结束时的源点云Q′应用空间变换TICP，效果如图5所示，记更新后的Q′为Q″即：

把经过空间变换TICP更新后的源点云Q″的OBB记为OBBQ″。从图5（a）可以看出，PointNetLK配准阶段结束时，盆栽点云的主体特征已经被对齐，但是不够规整，处于非凸优化问题中局部最优状态。利用能够刻画点云主体特征且规整的OBB能够帮助点云配准算法跳出局部最优，提升配准精度。

图5 中文盆栽点云OBB的对齐过程Fig.5 OBB alignment process of potted plant point cloud

通过观察图5（b），可以发现对齐OBBQ″和OBBP后的Q″和P，处于镜面对称状态。图5（b）所展示的处于镜面对称状态的源点云Q″和目标点云P，它们的对称面经过OBB的中心点，且平行于OBB侧面。点云Q″和P想要从镜面状态恢复到完全拟合状态，Q″需要以OBB的中心c∈ℝ3为中心，以表示OBB方向且平行于OBB长边的单位向量为旋转轴，旋转π个弧度，记此空间变换矩阵为Tmirror，则：

式（18）中之所以是约等于的关系，是因为噪声和传感器误差的存在。

至此，可以得到配准任务的最终空间旋转变换估计Tfinal：

在求取Tmirror的过程中，模板点云和源点云镜像对称的对称面可能有三种情况，即分别以表示OBB方向的单位正交向量为法线的平面。所以，Tmirror也有三种计算结果，从中挑选最佳Tmirror，使得P和Tmirror·Q″的拟合度最高。改进后的总体算法流程图如图6所示。

OBB能够更加规整地表达三维点云的宏观特征，如图7所示，为三维点云注册任务提供点云的全局信息。PointNetLK-OBB借助OBB避开PointNetLK语境下的局部最优，加快配准速度，提升配准精度。

2.3 PointNetLK-OBB的训练

为了保证PointNet能够从点云数据中提取到特征，需要训练PointNet分类网络。但是，仅训练PointNet，网络提取到的特征并不适合点云配准任务。为了保证PointNet能够从点云数据中提取到适合于配准任务的全局特征，为后序的高精度配准奠定基础，需要在Point-Net分类网络的训练基础上，训练PointNetLK以达到微调PointNet网络参数的目的。

图6 PointNetLK-OBB算法推理模型Fig.6 Reasoning model of PointNetLK-OBB algorithm

图7 盆栽点云的方向包围框Fig.7 Orientation bounding box of potted plant point cloud

PointNetLK之后的算法部分，没有使用PointNet“成像”函数Ψ和Ψ提取到的点云特征，而是直接用点云本身来完成最后的高精度配准，所以在训练算法模型时，只训练PointNetLK部分。

训练PointNetLK所使用的损失函数为PointNet“成像”函数Ψ提取到的源点云Q和目标点云P的特征差的二范数，以及由PointNetLK估计出的变换矩阵和真实变换矩阵之差的F范数，即：

3 实验

3.1 数据集

使用ModelNet40[29]作为实验数据集。ModelNet40是一个公开的数据集，共包含40个类别的CAD模型，每个类别由训练数据和测试数据组成。因为PointNetLK使用ModelNet40来测试和分析算法性能，所以使用该数据集，方便与PointNetLK进行比较。

3.2 不同初始偏转角度下的配准误差

为了验证结合方向包围框改进的PointNetLK算法PointNetLK-OBB的有效性，将本文方法与PointNetLK算法在ModelNet40上做对比实验，直观对比效果如图8所示。本文结果显示，结合方向包围框改进的PointNetLK在待配准点云偏转角度较大情况下，有效地降低了配准误差。

分别用PointNetLK和PointNetLK-OBB在ModelNet40的测试集上进行实验。为了观察算法在不同初始状态下的配准表现，在实验中用到的源点云和模板点云的初始偏转角度以10°为间隔，从0°到180°，均匀分布，位移为[0,0.3]之间的随机量。将估算得到的变换矩阵Tfinal和真实变换矩阵的逆(Tgroundtruth)-1之间的乘积Terror=Tfinal·(Tgroundtruth)-1作为算法评价标准。为了验证算法在数据集上的整体表现，使用同一偏转角度下的所有测试数据的平均误差来表示最终的算法性能。

图8 PointNetLK和PointNetLK-OBB配准结果对比Fig.8 Comparison of registration results between PointNetLK and PointNetLK-OBB

第一组观测数据为算法的平均位移误差。使用Terror中的t（见式（2））的模表示位移误差，由同一偏转角度下的所有测试数据的位移误差平均得到最终的位移误差。不同偏转角度下的最终位移误差如图9所示。

图9 不同初始偏转角度下的平均位移误差Fig.9 Average displacement error under different initial deflection angles

从图9可以看出，待配准点云间的初始偏差角度大于90°时，PointNetLK的平均位移误差在继续增大，而PointNetLK-OBB能够保持一个较低误差水平。

第二组观测数据为算法的平均偏转角度误差。使用δerror代替Terror（见式（3）），由δerror的前三个分量δ1、δ2、δ3的模表示旋转角度误差，由同一偏转角度下的所有测试数据旋转角度误差平均得到最终的角度误差。不同偏转角度下的最终角度误差如图10所示。

从图10可以看出，待配准点云间的初始偏差角度大于90°时，PointNetLK平均旋转角度误差逐渐增大，而PointNetLK-OBB能够保持一个较低的旋转角度误差水平。

图10 不同初始偏转角度下的平均旋转角度误差Fig.10 Average rotation angle error under different initial deflection angles

由于PointNetLK使用全连接网络PointNet(Ψ)仅提取到点云宏观轮廓特征，在待配准点云初始偏转角度较大情况下，至少存在一种非目标姿态下的点云特征是和目标姿态特征相同，导致LK算法在非目标状态时，停止更新转动参数（见式（8）），配准算法陷入新的局部最优，见图4。所以点云偏转角度越大，这种与目标姿态点云特征相同的非目标姿态越多，导致PointNetLK配准误差越大。

结合方向包围框改进的PointNetLK算法，借助PointNetLK跳出了传统配准算法的非凸问题，对齐了点云中的主体成分，利用方向包围框的规整性，在ICP算法的引导下，对齐了方向包围框，利用镜面对称效应，翻转点云，避开了由PointNet和式（8）和引起的新型局部最优问题，进而在点云偏转角度过大时，有效降低配准误差。

3.3 不同初始偏转角度下的配准时间性能

为了测试PointNetLK-OBB算法的时间性能，将其与原始PointNetLK算法在Intel Xeon 2.1 GHz CPU上进行对比。结果显示，本文算法的运行时间并没有显著增加，与原始算法的时间性能相差无几。

分别用PointNetLK和PointNetLK-OBB在ModelNet40的测试集上进行实验。在实验中用到的源点云和模板点云的偏转角度从0°到180°，以10°为间隔，均匀分布，位移为[0,0.3]之间的随机量。为了测试算法在数据集上的整体时间性能，使用同一偏转角度下的所有测试数据的平均运行时间来表示最终的算法时间性能，如图11所示。

从图11可以看出，PointNetLK-OBB和PointNetLK的运行时间随着初始偏转角度的增大而增加，且没有明显差异。结合图9和图10，可以发现，在高精度完成配准任务的前提下，PointNetLK-OBB运行时间仍然处于一个可接受的水平。

PointNetLK-OBB之所以能够保持较低的时间消耗，是因为源点云和模板点云方向包围框的规整性。PointNetLK-OBB可以快速使用ICP对齐包围框的八个顶点，并在待配准点云之间产生镜面对称效应。处于镜面对称状态的待配准点云，仅需依靠点云自身的旋转，便可完成高精度的点云配准。提升精度的同时，降低了算法的时间消耗。

图11 不同初始偏差角度下的平均运行时间Fig.11 Average running time under different initial deviation angles

4 结语

本文针对PointNetLK在待配准点云偏差角度过大时，陷入局部最优，损失精度的问题，提出了一种结合方向包围框改进的点云配准方法PointNetLK-OBB。该方法利用PointNetLK跳出了传统点云配准的非凸问题，完成了点云主成分的对齐，利用三维点云方向包围框的规整性，并在ICP的引导下，在点云间产生镜面对称效应，配合源点云自身的旋转，避开由缺乏卷积结构的PointNet引起的新型局部最优问题。

改进的配准算法PointNetLK-OBB在公开的三维数据集ModelNet40上与原始算法PointNetLK进行对比实验。实验结果表明，PointNetLK-OBB在待配准点云偏差角度较大的情况下，能够避开PointNetLK语境下的局部最优，保证时间效率的同时，高精度地完成三维点云配准。

针对本文的方法，有读者可能会提出用方向包围框、ICP及其产生的镜面对称效应，替代PointNetLK。本文认为，单独使用方向包围框、ICP及其产生的镜面对称效应，无法完成点云配准。方向包围框系列的方法，需要ICP的支持。而ICP最大的问题就是高度依赖待配准点云的初始位置，很容易陷入传统非凸问题。本文方法很好地解决了这个问题，利用PointNetLK跳出了传统非凸问题，又利用方向包围框的规整性避开了PointNetLK语境下的局部最优问题。

经过实验发现，目前，PointNetLK-OBB只能对尺度一致的点云进行配准，未来的改进可以利用方向包围框的伸缩，来完成尺度不一致的点云配准。

本文算法目前可以应用于AR医疗，未来还可以扩展至机器人路径规划、自动驾驶等应用和三维重建等领域。因此，本文算法是一项有意义的研究。