基于图像和激光的多模态点云融合与视觉定位

第 1 章引言

1.1 研究背景及意义

人工智能(ArtificialIntelligence)在过去十几年来的蓬勃发展让现实生活中的许多领域变得日趋无人化与智能化：增强现实(AR)使人能够和虚拟环境进行互动；自动驾驶技术(self-drivingtechnology)的落地与应用使得道路更加智能化，提升了道路运输的安全性与便利性[1]；智能机器人的发明，一方面如扫地机器人的出现使得智能家居领域得到发展；另一方面在工业领域移动机器人的出现使得人们从重复性高或危险性高的工作当中得以解放[2]。图 1.1 列举了常见的几种智能化领域。

(a)增强现实 (b)自动驾驶

©扫地机器人 (d)移动机器人图 1.1 近年常见智能化领域

以上几种智能化技术的应用，离不开对于场景的构建与定位工作。场景构建是指对一个真实的环境采用特定的模型进行数学描述，在计算机视觉领域，一般通过构建场景的点云模型来进行建模，一个准确、高精度的场景点云模型能够为定位系统提供更好的基础。定位是指移动机器人通过自身的传感器获取数据，对外界环境信息进行感知与处理，通过图像处理技术或点云处理技术来计算移动机器人的位置。定位任务又可以分成两个部分：绝对定位与相对定位。绝对定位是指计算自身在环境中的绝对位置，而相对定位是指在机器人的运动过程中估计每一时刻与上一时刻的相对运动，从而恢复出整个运动轨迹。

在图 1.1 的各个领域中，场景构建与定位都发挥着重要作用。在自动驾驶领域，一个准确的定位能够提升无人汽车的安全性；一个高精度地图的构建能够提升用户在增强现实上的体验；同时一个鲁棒且准确的建图定位算法也能够提升工业机器人的工作效率与准确性。

如今，在处理建图与定位任务时主要采取两种方向：第一，同时建图与定位。这里的定位解决的是相对定位，即机器人在环境中移动的时候，利用环境信息特征，一边推测自己的相对位姿从而恢复轨迹，一边进行地图构建，当机器人完成整体环境的探测时，也完成了地图的构建。同时建图与定位算法被称为 SLAM 算法(SimultaneousLocalizationandMapping)。

第二，先建图后定位。这里的定位指的是绝对定位，不同于 SLAM 算法，它需要机器人首先对全局环境进行了解，从而将其放入环境中时，能够准确定位出机器人的绝对位置。对于建图任务，常用的方法有基于单目视觉的 SfM(Structure-from-Motion)算法，能利用图像稀疏地构建环境三维模型。对于室内定位任务，常采用的方法是提前在场景粘贴视觉 marker，并在重建地图中进行标记，以此来进行检测与定位[3-4]，常用的视觉 marker 有 Aruco[5]等；在室外环境中，通常利用 GPS 信号进行辅助定位[6]。

(a)双目相机 (b)RGB-D 相机 ©激光雷达图 1.2 实验设备

随着技术的进步，各种传感设备正在不断更新，如图 1.2 所示，双目相机、RGB-D 相机、激光雷达的出现使得对于环境数据的采集变得更加便捷、数据类型更加多样。但尽管如此，在重建与定位的算法方面仍面临着不少挑战，主要有如下几点：

1.大场景的精确重建具有挑战性。对于大场景的重建问题，通常有两种解决方式，第一种是依靠传统的 SLAM 或 SfM 算法进行一次性重建，但在大场景构建中前者会存在尺度模糊与漂移问题[7]，后者由于搜索数据的增加造成算法效率低下。另一种解决方式是先对局部场景进行构建，再进行融合，这需要一个有效的点云融合算法进行支撑。

2.视觉定位需要具有鲁棒性。在利用视觉进行定位的算法中，常常面临着不够鲁棒的问题：因为受时间及环境变化的影响，建图与定位时场景中的光照细节可能发生较大的变化，这个容易给传统图像匹配算法的性能造成影响。

3.传感器对定位性能的限制。采用激光雷达设备，其实时性较差，并且成本较高，不利于长期性、实时性的定位需求，而利用传统的单目相机进行视觉定位，由于视野较窄、视角方向单一，在场景中容易由于受到遮挡，或环境特征不够明显时出现定位丢失的现象。根据以上挑战，本文提出了一套对于大场景的建图与定位算法。首先它采用了从局部到整体的思路来减少漂移现象，先选择合适的方式构建场景局部点云，然后对不同局部的点云进行有效地融合，完成重建任务。在视觉定位上，选择了全景相机作为传感器设备，能够覆盖比传统相机更广的范围，捕捉比激光雷达更远的视觉信息；并为了克服跨模态图像的差异，采用了基于神经网络的特征匹配方法，提升了定位过程中的鲁棒性。

1.2 研究现状

1.2.1 视觉三维重构

目前基于视觉三维重构主要有两个流行的方向，第一个是采用视觉 SLAM 的方法，按照一定的采样率对场景进行连续的图像拍摄，并依靠算法在定位的同时恢复场景的整体图像；第二个是采用 SfM 方法，提前在场景中拍摄大量无序照片，通过搜索匹配的方式发现重叠图像，进而恢复场景模型。本小节分别对于这两种视觉三维重构方法的研究现状进行了总结。

1.SimultaneousLocalizationandMapping

SLAM 的整体框架如图 1.3 所示，其架构包括前端和后端两个部分[8]。其中前端利用传感器的数据，估计出传感器的运动，从具体实现上，该模块首先从传感器数据中提取特征，然后寻找特征之间的关联，从而从短期上看可以估计出相邻两帧传感器数据之间的运动，从长期看能够通过判断新特征是否与之前数据中的特征相似来进行回环检测。由于传感器数据存在噪声、特征提取与配对不能做到完全准确，因此在估计运动时不可避免地会出现误差，这一误差会随着传感器的运动而逐渐累积，而 SLAM 框架中的后端则是通过优化算法，来消除累积误差，从而得到更为准确的地图及运动轨迹。

图 1.3 典型 SLAM 系统中的前端与后端

若上图中的传感器为相机，则相关的 SLAM 算法则被称为视觉 SLAM(VSLAM)，即利用图像特征进行建图与定位。本小节以 VSLAM 为例，介绍几种当前常见的算法及框架。

MonoSLAM 是第一个仅利用单目相机进行实时建图与定位的框架[9]。MonoSLAM 能够对图像中的特征点进行提取，并追踪其运动，同时利用扩展卡尔曼滤波器(EKF)的方式对当前状态进行更新，并调整 landmark 状态的均值和协方差矩阵。根据 EKF 的原理，特征点的位置服从三维空间中的高斯分布，通过连续观测，能够降低位置的不确定性，从而使定位精度逐渐收敛，这一算法框架适用于小范围的场景，具有较好的实时性。

图 1.4 MonoSLAM 特征检测与地标效果图

SLAM 技术的第二个里程碑是 Kleinetal.[10]提出的 PTAM。PTAM 算法第一次将 SLAM 任务分解为前端和后端两个部分，这为之后许多 VSLAM 的系统设计提供了参照。不同于传统算法中以滤波方式进行优化，PTAM 算法采用非线性优化作为后端算法。同时为图像序列标记关键帧，避免了对所有图像的冗余处理，提升了轨迹估计和地图重建的速度，保证了算法的实时性。PTAM 提出后，有基此算法开发的 AR 软件问世，能够利用相机对环境中的平面进行感知，从而逼真地在真实环境中展示虚拟物品。

图 1.5 PTAM 算法效果图

2015 年，Mur-Artaletal.[11]提出了 ORB-SLAM，该算法在 PTAM 的基础上能够自动生成初始化地图，并通过特征比对判断闭环，同时在关键帧选择与特征提取方法上进行了优化，得到了实时性和精度更好的效果。ORB-SLAM 的框架如图 1.6 所示，可以分为跟踪、建图、重定位与闭环检测四大部分。在跟踪阶段，ORB-SLAM 采用了 ORB 描述子[12]对图像特征进行描述，能够更好地提升特征提取的速度，同时具有光照不变性，因此更加鲁棒；在建图模块中，算法通过特征点不断生成新的局部地图点，筛选后插入点云地图中，该部分同时实现了关键帧的筛选以及去除功能；闭环检测模块中，首先对关键帧记性检测判断回环，当回环出现时，使用地图融合技术对地图中重复的点云进行融合，并采用 EssentialGraph 的方式对回环进行优化，使得地图更好地闭合。最终，该算法实现了在不依赖多线程与 GPU 的情况下，达到了 30fps 的追踪速度，因此适用于实时 SLAM 中。在 ORB-SLAM 的基础上，作者还于 2017 年发布了基于双目相机与 RGB-D 相机的 ORB-SLAM2 算法[13]。

图 1.6 ORB-SLAM 流程图

近年来，得益于深度学习的不断发展与进步，不少学者将其融入到传统的 SLAM 算法之中。该类算法的思路往往是利用深度学习的方法对 SLAM 框架中的一个或多个模块进行替换，达到更加鲁棒的效果。

CNN-SLAM[14]算法利用 CNN 网络对关键帧图像的深度进行了估计，并与传统 SLAM 算法中得到的深度进行融合，从而优化特征点的位置，同时也使用神经网络对图像的语义信息进行跟踪，得到了语义连贯的重建结果，CNN-SLAM 在低纹理区域等场景中得到了比传统方法更鲁棒和准确的结果。

图 1.7 CNN-SLAM 流程图

DeToneetal.[15]采用深度学习，替换了传统 SLAM 框架中的特征点提取和匹配模块。首先利用 MagicPoint 网络对单帧图像的关键点进行提取，相比于传统特征描述子，该网络在图像有噪声的情况下能够更鲁棒地提取到有用的关键点。第二个网络是 MagicWarp，它仅以 MagicPoint 网络输出一系列图像点对的坐标作为输入，在不依赖于局部点描述子的情况下估计 3×3 的单应性矩阵，进而恢复相机的转移，该算法同样在单核 CPU 上实现了 30fps 的实时 SLAM。

图 1.8 面向几何的深度 SLAM 框架

2.Structure-from-Motion

在 SfM 方面，来自华盛顿大学的研究人员 Snavelyetal.[16]最早为这一算法提供了流程上的基础。在 PhotoTourism 项目中，他们从各大照片网站中爬取同一个景点的不同照片，利用 SIFT 算法[17]提取图像中的描述子，然后对图像中的特征点两两进行匹配，利用对应点的几何关系重建出场景模型。利用该算法，他们对意大利罗马的特莱维喷泉和中国的万里长城进行了场景重建，如图 1.9 所示，都证明了这一流程是可行的。

(a)特莱维喷泉 (b)长城

图 1.9 PhotoTourism 场景重建

对于 PhotoTourism 中的算法，由于其需要进行大量的匹配和搜索，因此复杂度达到了(^4)，其中为照片的数量。随着 VisualSFM 算法[18] 的提出，这一复杂度被下降到(^2)，同时也对重建的精度得到了较好的保留。得益于 SIFT 算法在 GPU 上的运行[19]，该算法的效率更是得到了较大的提升，在此方法发表后，基于 VisualSFM 算法的 GUI 应用也被开发了出来，方便了重建的流程。

以上 SfM 算法均为增量式的重建算法，Moulonetal.[20]在 2013 年提出了全局式的 SFM 算法，相比于增量式的 SfM 中一边重建一边进行局部优化带来的效率低下、并且容易出现漂移性的问题，全局式的 SfM 算法先对所有图像计算关键点，利用对应关系求取变换，最后通过一次性的优化对整体进行调整，这大大提高了算法效率，但该算法容易受到一些噪声图像的影响，因此相对而言鲁棒性较差。

图 1.10 基于全局 SfM 算法的稠密重建

Cuietal.[21]将两种 SfM 方法结合，提出了混合式 SfM 算法，其框架图所示。该算法首先进行图像相似部分的检测，在全局上估计相机的平均运动，然后再以增量式的方式细化每一个相机的位置，最后经过一次优化来完成地图的构建，通过实验也证明了这一算法框架在效率、准确性和鲁棒性上的优势。

1.2.2 点云配准

点云配准问题可以用以下数学模型来进行表述。令和为分别为两个点云的坐标矩阵，其中,分别是矩阵

图 1.11 混合式 SfM 算法流程图

X 和中的第和第行，代表两个点云中第和第个点，假设和之间有个点相对应，则点云配准的目标是估计刚性变换函数（可分解为旋转矩阵 ∈ (3) 和位移向量 ∈ 3），使得通过变换后两个点云能够最好地对齐：

其中

代表 X 和变换后 Y 之间的误差。在点匹配关系已知的情况下，转移矩阵能够很好地被计算出；在转移矩阵给定的情况下点对关系也能很好的估计，但是这两个问题需要同时求解时就带来了很大的挑战[22]。

点云配准领域中，最传统且最被广泛使用的莫过于 ICP 算法[23](IterativeClos-estPoint)。ICP 是一种迭代式的算法，不断通过搜索最近匹配来优化旋转矩阵与平移向量，其算法可以分为以下几个步骤：

1.搜索两个点云中点到点距离最近的若干个点对；

2.根据搜索的点对求解两个点云之间的 R 和 t；

3.利用计算的位姿将源点云进行变换，判断变换后两个点云的误差是否小于给定阈值；

4.如果误差过大，则在变换后的两个点云之间继续搜索距离最近的点对，然后重复 2,3 步直至误差达到阈值或达到最大步数。

ICP 算法根据点到点的距离关系假设了匹配对关系，然后通过迭代算法更新匹配对关系并求解转移矩阵。然而在迭代的过程中，算法不能保证收敛到全局最优，当给定的初值与真实情况相差较大时，ICP 算法往往不能获得理想的结果。在 ICP 算法提出之后，也有不少算法在 ICP 的基础上进行了改进。例如 Effi-cientICP[24]提出了几种不同的策略来提升 ICP 算法的速度；同时，Chenetal.[25]将传统 ICP 算法中点到点距离的度量改为了点到平面距离的度量，即最小化一

图 1.12 ICP 算法流程图

个点云中的点到另一个点云对应平面的垂直距离之和；沿着这个思路，Forstneretal.[26]将平面到平面距离运用到了 ICP 算法当中，都使得 ICP 算法的性能得以改进。

传统的点云配准方法在过去十几年中取得了相当大的进展，但仍然在性能上存在瓶颈[27]。在最近几年，许多基于深度学习的算法被提出，这些方法按照时间顺序如图 1.13 所示，它们在 Model40[28]、KITTI[29]等公开数据集上获得了比传统方法更好的性能。

图 1.13 基于深度学习点云配准网络概述

2017 年，Qietal.[30]提出了 PointNet 网络，能够用从点云中提取特征，将许多之前基于图像的任务：如分类、语义分割和目标检测等，应用到了三维点云上。2019 年，PointNetLK[31]将 PointNet 网络与 Lucas–Kanade 光流算法[32]相结合，首次在不依赖于初始点对输入的情况下基于深度学习配准点云，从源点云与目标点云提取到的特征向量可以看做与图片特征向量相同，然后利用与图像相似的配准方法对两个点云进行配准。图 1.14 展示了这一网络的流程，分别是利用 PointNet 从源点云和目标点云中提取到的特征，然后通过计算雅可比矩阵并利对特征进行一阶泰勒展开，来求解得到 R 和 t，同时使用了逆向 L-K 算法大大提升了算法的计算效率。

图 1.14 PointNetLK 网络架构

与 PointNetLK 类似，PCRNet[33]采用 PointNet 进行特征提取，其结构图图 1.15，不同的是 PCRNet 在特征对齐模块中，使用了一种数据驱动的技术。首先连接两个全局特征，然后应用五个全连接层，以及输出层来直接进行位姿的估计。该算法在效率上比 PointNetLK 更加优秀。

图 1.15 PCRNet 网络框架

在 DCP 网络(DeepClosestPoint)[34]中，作者首次将 Transformer 结构[35]用于点云配准领域。如图 1.16 所示，源点云和目标点云同样先通过一个共享参数的特征提取模块，例如 PointNet，分别得到两个点云的特征，然后利用 Transformer 将这两者之间关联起来，得到两个点云之间的 softmatch 矩阵，最后通过 SVD 分解的方式，计算得到两个点云之间的变换矩阵。

图 1.16 DCP 网络框架

总的来说，目前基于深度特征的点云配准算法性能一方面受特征提取器的影响，提取到的特征描述符越准确、对运动的灵敏度和对噪声的鲁棒性越高，则方法可能越好；另一方面对准模块决定了配准效果的上限，该模块在最终结果的生成上起着重要的作用。

1.2.3 图像特征描述

图像特征描述是指对于一张图像中感兴趣的点或图像块进行特征表述，一个好的描述符应该具有旋转不变性和尺度不变性，即空间中相同点在不同图像上的投影应该具有欧氏距离相近的特征向量，即时两张图像之间有旋转变换或者尺度变换。

为了实现尺度不变性，SIFT 算法[17]为输入图像构建了不同分辨率下的金字塔并对每层金字塔进行差分，在 DoG 空间中寻找局部极值点作为关键点[36]，并对关键点进行定位与梯度计算，根据梯度构建直方图并确定主方向，根据主方向保证描述符的旋转不变性，最后利用插值后的梯度直方图作为图像关键点的特征表示。

PCA-SIFT[37]在 SIFT 算法的基础上进行了改进，利用主成分分析(PCA)算法[38]对每个点附近像素点水平梯度和垂直梯度构成的特征向量进行降维，以此替代了 SIFT 算法中直接用梯度直方图的特征表示，降维的映射矩阵通过对同一类图像中的大量特征的学习而得到，这一算法能够减少描述子的维度，提升匹配的效率。

SURF 算法[39]也是对 SIFT 的改进，它首先对求取图像的 Hessian 矩阵，利用 Hessian 矩阵挑选出图像中变化最剧烈的点作为特征点；不同于 SIFT 用金字塔模型来构造尺度空间，SURF 对于同一大小的图像应用不同大小的盒式滤波器来进行模糊化。在特征点定位和描述子生成的方法上基本与 SIFT 保持一致，但是整体提取的速度更快，并且保持了旋转尺度不变性。

图 1.17 SIFT 和 SURF 图像金字塔原理对比

在卷积神经网络的发展和帮助下，近年来人们开始利用 CNN 进行特征描述学习。其中代表网络结构之一是 MatchNet[40]，它的结构如图 1.18 所示，由一个深度卷积网络和一个具有三个全连接层的网络组成，前者能够从图像块(patch)中提取特征，后者用来计算提取到特征之间的相似性。MatchNet 显著提高了基于手动描述符的匹配结果，显示了 CNN 在描述符学习方面的巨大潜力。

图 1.18 MatchNet 网络结构图

1.2.4 视觉定位

目前，利用视觉图像在先验激光点云中的定位问题是自动驾驶等领域的重点，这一问题的难点在于 2D 图像与 3D 点云之间的模态存在较大差异，难以构建一个统一的描述符进行特征提取与匹配。根据已有研究研究，克服这两者之间差异的方法大多是将这两者转换到同一空间中，即统一在 2D 空间或 3D 空间中完成匹配[41]。

第一种定位方式是在 2D 空间中完成匹配定位。例如在前文中介绍的 SLAM 算法，能够通过提取特征描述子的方式完成局部的稀疏点云三维重建，并且通过图像特征来进行回环检测并减少漂移，同时还有 SfM 算法，也是通过 2D 匹配的方式完成大规模场景的三维构建。但是，图像特征对于光照条件比较敏感，在光线变化比较大的场合，难以完成鲁棒的图像匹配。Wolcottetal.[42]针对自动驾驶领域提出了一种新的算法，如图 1.19，利用先验 Lidar 地图，生成一些合成的针孔相机图像，并与车载实时图像利用标准化互信息(NMI)进行评估以生成定位。

图 1.19 基于标准化互信息的图像定位算法

第二种定位方式是在 3D 空间中进行匹配以计算相机位姿。如图 1.20 所示，Gaweletal.[43]首先通过视觉相机和 IMU 信息，采用 SfM 构建局部三维稀疏点云，然后利用三维结构描述符与稠密的离线地图进行 3D-3D 匹配。同时，也有学者直接利用立体相机，构建局部点云信息并匹配离线点云，然后将匹配结果耦合到 VO 和 VIO 系统中，来优化摄像机位姿[44]。但是如上方法也存在着问题，SfM 算法无法为地图提供尺度信息，因基于视觉重建的稀疏局部点云与离线地图点云之间可能无法找到对应的匹配；立体相机得到的局部点云数据量较大，因此 3D 匹配的耗时可能大大增强。

图 1.20 基于 3D-3D 匹配的视觉定位原理图

1.3 本文工作

如本章第一节中介绍，目前建图与定位领域存在着许多困难与挑战，而建图与定位的精确程度又是紧密耦合的，一个高精度的地图是准确定位的基础。针对这些挑战，本文提出了一种可行且鲁棒的建图和定位的的算法流程，实现了对于大场景的离线建图与基于视觉的在线定位功能，其框架如图 1.21 所示。

本文算法将场景重建和定位两个部分分离开来，把复杂且计算开销大的点云匹配融合等建图相关工作放在离线部分完成，对于较为简单的图像匹配、矩阵求解等定位相关工作放在在线部分完成。

从数据采集设备来看，该算法为建图提供了不同的设备选择，既有成本低的单目相机、也有准确度较高的激光雷达与激光扫描仪，它们在数据采集与建图中

图 1.21 本文建图-定位算法框架

各具有优劣，实际使用中可以根据需求选择合适的设备和算法；定位的设备采用了全景相机，这主要是从视野的角度出发的，因为全景相机能够无死角地将周围的所有环境图像特征融入到一张图片内，因此可以给定位提供更多的参照。

在建图环节中，本文采用了先局部后整体的方式，这主要是为了减少传统算法在大场景建图中容易出现的漂移现象，同时提升建图过程中的效率。利用 SfM 算法或 LOAM 算法，能够从单目相机或激光雷达采集到的数据中恢复场景的局部地图。若干局部地图进行两两匹配，通过一个由粗到精的匹配算法来进行融合，这一算法中结合了传统的点云特征描述子，同时也结合了深度学习对粗匹配点对进行筛选，在不依赖于初始转移估计的情况下获得了比传统 ICP 算法更好的效果，由此得到了全局场景的三维点云。

在定位环节中，为了避免在 3D 空间中耗时更大的匹配，本文选择将点云与全景图像统一到 2D 空间中进行匹配，根据这个思路来解决图像与激光之间的跨模态匹配问题，但这涉及到两个问题：第一，点云如何投影到图像中，并尽可能保留更多的特征；第二，全景图像带来更广阔视野的同时也会带来较大的畸变，对特征描述造成影响。针对这两个问题，本文分别设计了点云平面映射与全景畸变校正两个模块。同时，对于建图与定位环境光照条件变化的挑战，本文采用了深度学习进行描述符学习，取得了比传统特征匹配方法更鲁棒的结果。最后通过对极约束求解出查询图像在场景中的位置坐标。

1.4 论文结构

本文主要由 5 个章节构成，主要内容如下：

1.第 1 章介绍了三维重建与定位技术的背景与内容，分析了此领域研究的意义与难点。同时对近年来 SLAM 技术和点云融合算法进行了简要介绍。

2.第 2 章介绍了本文采用的局部三维重建方法，并完成了利用 SfM、LOAM 以及图像激光扫描仪对大礼堂场景的局部重建实验，并进行了对比。

3.第 3 章介绍了本文提出的一种由粗到精的点云配准流程，能够融合局部点云生成场景全局点云地图，并分别在室内外自己采集的数据上进行了实验与分析。

4.第 4 章介绍了一种视觉定位方法，能够利用先验点云地图进行绝对位置的确定，并通过实验证明了该方法的可行性。

5.第 5 章对本研究的成果进行了总结，并提出了对未来可能的改进方向进行了展望。

第 2 章局部三维场景重建

2.1 引言

相比于直接对大场景进行地图构建，将其分解成若干个局部重建的子任务是一个更好的选择，这样通过一个有效的融合算法，便能够减少重建过程中的漂移现象，同时每个子任务的处理效率也能有所提升。在局部重建任务中，重建的准确性和稠密程度会对后续全局地图融合以及在其中的定位产生重要的影响。本章分别采用了三种方法对于局部场景进行重建，分别是基于视觉的 SfM 算法、基于激光的 LOAM 算法、以及基于 Leica 三维扫描设备的重建方法，并对它们的重建效果进行分析比较。

2.2 基于视觉的 SfM 三维重构

2.2.1 算法简介

SfM 算法(Structure-from-Motion)能够从一系列在场景中拍摄的无序 2D 图像中恢复场景的三维结构。目前 SfM 算法主要有三个分支，分别是增量式重建[45]、层次化重建[46]与全局式重建[47]。其中，增量式重建是最流行的策略，该算法的流程图如图 2.1 所示[48]。

图 2.1 增量式 SfM 算法流程图

SfM 算法主要包括两大部分，首先是点对搜索，然后是增量式重建。在点对搜索模块中，场景的无序图像首先通过特征提取算法，获得每一张图片中的局部特征，该特征应该具有旋转不变性与尺度不变性，使得其在不同角度和不同距离拍摄的照片中具有相似的特征向量。在匹配阶段，需要通过提取的特征寻找不同图像中相重叠的部分，获得重叠的图像对和它们当中的特征匹配对。但是提取到的图像匹配对和特征匹配对中可能含有大量的噪声，因此需要进行验证，该算法通过估计两个图像的单应性矩阵来获得这两帧图像之间的转移，如果该转移能够映射足够数量的特征，则被视作是有效的，最后得到了通过集合验证的图像对及其特征点对。

在增量式重建模块中，算法首先从图像密集的区域中选择两张作为初始化，这是为了提升重建的鲁棒性与性能；其次，利用新图像和已有模型之间的 2D-3D 匹配对，能够通过求解 PnP 问题[49]将新的图像的位置配准到已有模型之中，同时估计新图像的相机内参，对于之前模型未覆盖到的部分，一旦有两张以上的图片观测到其中相同的特征点，就可以利用三角化方法将新的特征点对齐到整体模型中。最后采用光束法平差(BundleAdjustment,BA)[50]对配准结果进行优化，该方法的优化函数是每个特征点在图像上的重投影误差，据此对相机参数的估计和特征点位置的估计进行优化和更新。

2.2.2 COLMAP 简介

COLMAP 软件是基于 SfM 算法的开发的场景重构软件，它为通过自己采集的数据图像生成场景模型提供了便利的操作及 GUI 界面，只需要输入图像数据文件夹，就能够自动完成特征点提取匹配、以及增量式重建的流程，其界面如 2.2 所示。

图 2.2 COLMAP 软件界面

2.2.3 实验及结果

本节中采用的数据集为在一个晴朗的午后于大礼堂南面使用单目相机拍摄的 17 张平面图像图片，分辨率为 1440×1080 像素，如图 2.3 所示，分别编号为 0-16。

图 2.3 大礼堂图像数据集

采用 windows10 平台，COLMAP3.6 无 cuda 加速版本软件，对以上 17 帧图像进行重建，其结果如图 2.4 所示，其中红色四边形为相机成像平面的位置，其余点为利用 SfM 算法得到的大礼堂模型，结果中共有 4121 个点。

图 2.4 大礼堂场景 SfM 算法重建结果

2.3 基于激光雷达的 LOAM 三维重构

2.3.1 算法简介

随着硬件水平的不断发展，激光雷达的尺寸和重量不断减少，这给基于激光雷达的定位建图技术提供了很好的基础；另一方面，激光雷达对于场景的探测不依赖于环境的光线条件与场景中的光学纹理信息，因此相比于基于视觉重建的 SLAM 算法，更适用于更暗场景的重建，同时也能带来更高的精度。

Zhang et al.[51] 最早提出了以 6DoF 运动的两轴激光雷达进行场景重建的 LOAM 算法(Lidar Odometry and Mapping)，图 2.5 展示了这一算法的流程。是当前通过雷达获得的扫描，首先与之前 1 帧的点云的点云进行配准，得到。里程计模块通过比对相邻两帧的扫描来估计雷达的运动，同时纠正中失真，该模块以 1Hz 的频率输出无失真的，以 10Hz 的频率对输出雷达的运动。前者经过 Mapping 模块将无失真的点云匹配并对其到已有地图上，最后结合这两者的变换，以 10Hz 频率输出最终的变换估计。

图 2.5 LOAM 算法流程图

为了更加高效地配准两帧扫描的点云，LOAM 算法从每次扫描点当中选择两类点作为特征点。第一是角点，即位于扫描线上变化比较剧烈的点；第二类是平面点即线上曲率较小，变化缓慢的点。如图 2.6 所示，对于角点，可以看做是位于真实世界中两个平面交线上的点，而对于平面点可以看做是真实世界中某一个平面上的点。LOAM 算法通过优化当前帧角点到上一帧对应直线的距离和当前帧平面点到上一帧对应平面的距离，来进行位姿优化与运动估计。

图 2.6 角点与平面点示意图

2.3.2 实验设备

为了能更方便地在场景中实现控制和数据采集，本实验将一个移动运载底盘组装成小车系统，如图 2.7(a)，其最大速度为 20km/h，即 5.56m/s。同时在小车上

(a)实验小车 (b)VLP-16 激光雷达图 2.7 实验设备

搭载了 16 线激光雷达作为数据传感器，如图 2.7(b) 所示，能够实现 360 的在线扫描，其规格参数见表 2.1。

表 2.1 VLP-16 规格参数

2.3.3 实验及结果

利用上述设备，遥控小车在大礼堂门口的空地前走了一段距离，共拍摄 300 帧点云数据。利用 LOAM 算法生成了该区域的局部点云模型，结果如图 2.8 所示，其中白色的点云为重建结果，彩色的点云为激光雷达最后一帧的扫描数据，绿色线条为小车在地图中走过的轨迹。由于激光雷达扫描范围更广，并且相比于图像特征能够得到更多的点信息，因此局部重建结果更加稠密，在本次实验中，最终点云中共含有 164145 个点。

图 2.8 大礼堂场景 LOAM 算法重建结果

2.4 基于激光扫描仪的三维重构

2.4.1LeicaBLK360

普通单目相机拍摄出来的照片，含有丰富的 RGB 颜色信息，但容易受到环境光照条件的影响；而采用普通激光雷达得到的数据，又由于只含有反射强度信息而缺乏颜色，因此含有的信息更少。而采用如图 2.9 所示的激光扫描仪能够较好地平衡这两者的缺点，利用激光雷达与相机同时从环境中获取颜色与点云信息。

图 2.9 LeicaBLK360 激光扫描仪

该设备的参数如表 2.2 所示，相比于 VLP-16，在一次扫描中它能够采集到数量更多、范围更广的点，但相应采集时间也更长，并且数据采集过程中要保证设备静止。

表 2.2 LeicaBLK360 规格参数

2.4.2 扫描结果

利用该扫描设备，以中等精度的分辨率，同样在大礼堂门口区域对其进行局部场景扫描。其结果如图 2.9 所示，总共扫描得到 6136121 个点，各个点中包含了 xyzrgb 信息。

图 2.10 激光扫描仪重建结果

2.5 本章小结

本章分别采用了纯视觉方法、纯激光雷达与基于视觉 + 激光雷达的方法，对于大场景中的局部区域进行了重建实验，实验结果表明，这三种采集设备与重建方式各具有优缺点，其汇总见表 2.3。

表 2.3 局部场景重建方法优缺点对比

重建算法	SfM	LOAM	图像激光扫描法
设备需求	单目相机	激光雷达	Leica 激光扫描仪
设备价格	便宜	较贵	昂贵
采集时间	较短	较短	长
重建时间	长	较短	无需额外重建算法
重建精度	稀疏	一般	稠密

对于 SfM 算法而言，其最大的优点就是设备价格便宜，仅需使用单目相机就可以完成，并且采集成本较低，仅需对场景的不同角度进行拍照即可完成，但同时带来的代价是重建时间较长，因为特征的匹配搜索需要耗费大量的资源，并且重建精度较低；对于 LOAM 算法而言，其采用的激光雷达设备较为昂贵，但数据采集方便，仅需遥控小车即可完成，同时能够实时地完成地图构建任务；对于图像激光扫描法而言，其优点在于得到的点云非常稠密，并且不需要进行额外算法即可得到局部的点云数据，其缺点在于设备昂贵，并且数据采集的时间成本较高。

因此，在实际的局部重建中，需要根据成本及精度要求，选择合适的算法。

第 3 章基于由粗到精的点云配准

3.1 引言

上一章中探讨了三种局部地图构建的方法。但是，不管基于单目视觉的 SFM 算法，还是基于激光雷达的 LOAM 算法，在重建大规模场景点云时，需要与已有点云进行大量搜索和匹配，导致重建时间随场景规模增大而增加。因此，在本章中提出一种点云拼接算法，能够利用以上各算法重建出的局部场景点云，完成整体场景的构建。同时，本研究利用激光扫描仪采集到的数据，对此配准算法的效果进行了测试。

目前大部分传统的点云配准算法，如 ICP 等，需要先提供初始位姿，然后根据初始位姿进一步优化。这些算法有如下三大问题：第一，初始位姿的求取本身就是一项充满挑战性的工作，所以现实中采用相关算法往往需要预先手工对点云进行粗对齐；第二，从传感器中采集到的点云数据不可避免存在噪声，传统算法往往对于噪声比较敏感，容易产生错误的估计；第三，在场景点云匹配中存在着部分重叠的问题，即许多源点云中的点在目标点云中无法找到对应。这些问题和挑战共同限制了传统点云配准算法的性能。

受 Paisetal.[52]的启发，本研究尝试使用基于深度学习的 3DRegNet 网络解决点云配准任务，但该网络需要预先给定点云之间的对应关系。因此本文融合传统点云特征描述子提取与深度学习配准框架，提出了一个由粗到精(coarse-to-fine)的配准算法。该算法流程如图 3.1 所示，首先对源点云和目标点云中的每一个点
提取特征，特征维度为 Feature，然后在特征空间中对点进行匹配，得到粗配准点对(1, 1), , (, )，然后将点对关系输入 3DRegNet 网络，以进一步优化配准结果，输出两个点云之间的转移矩阵。

3.2 点云粗配准

3.2.1 局部特征描述子

在 2D 图像匹配领域，寻找图像之间对应的方法是构建特征描述符，根据每个关键点及其领域的信息提取特征向量。这些基于 2D 图像的描述子设计给基于 3D 点云的描述子设计提供了思路，目前最经典的点云特征提取算法是 Rusu

图 3.1 点云配准算法流程图

etal.[53]提出的 FastPointFeatureHistograms(FPFH)算法，此算法能够保证旋转不变性，同时兼顾了鲁棒性与算法效率。本章中利用了此算法提取对点云进行了特征提取。

FPFH 特征在 PFH 特征[54] 的基础上进行了效率优化。对于点云中的每个点(query point)，选择其半径范围内的近邻点，这个点分别与 query point 构成个点对(, )，对于每个点对，首先构造局部坐标系：

其中表示点处的法向量，× 表示外积。点对之间的角度特征用 < , , > 表示，定义如下：

对于每个查询点的个匹配对，将三个角度特征分别划分为个区间，统计
落在每个区间的数量分布直方图，然后将其拼接得到单个查询点的 SPFH 特征，
其维度为 3，FPFH 特征可以看做查询点与近邻点 SPFH 特征的加权和

其中为查询点和邻点的距离。图 3.2 展示了每个查询点的影响区域范围，每个查询点与灰色半径范围内的个点相连，查询点的 FPFH 特征由灰色圆圈中的 + 1 个点的 SPFH 直方图加权而来，其中图上标号为 2 的连接对 FPFH 特征贡献了 2 次。

图 3.2 FPFH 影响区域图

3.2.2 基于点特征的全局粗匹配

FPFH 算法能够利用点云中间的空间关系，将点用高维向量进行特征表示，这给寻找两个点云之间的点配对关系提供了工具。本章采用了 KD-Tree[55]方式为源点云与目标点云之间建立搜索树，然后通过 RANSAC 方式[56]排除错误的匹配对，同时估计出两个点云之间的旋转矩阵与平移向量。

KD-Tree 是为维向量数据构建的二叉搜索树，在所有非叶子节点上用过该节点向量的超平面将维空间分割成两个部分，被分割的两个子空间中的点分别作为该节点的左子树和右子树。利用 KD 树能够将搜索复杂度降至(log )，提升搜索效率。

图 3.3 展示了从源点云提取特征并构造 KD-Tree 的示意图。树中的每一个节点为点云中各点的 FPFH 特征，深度为的非叶子结点的分割平面为与第个维度轴垂直的超平面。

图 3.3 KD 树构造示意图

根据源点云与目标点云的 FPFH 特征与 KD 树，可以对于源点云的任意一点寻找其在目标点云中的对应，然而这样的对应并非完全准确的，在正确的配对(inlier)之外可能产生较多的错误配对(outlier)。通过 RANSAC 算法能够减少 outlier 对配准的影响，达到较好的效果。

基于 RANSAC 算法的点云粗配准流程如算法 3.1 所示，该算法重复地从源点云中随机抽样 4 个点坐标，再在目标点云中根据 FPFH 特征寻找对应点，并利用这 4 个点的三维坐标估计转移矩阵，仅仅计算四个点转移矩阵是容易的，可以采用 SVD 分解[57] 的方法来快速求得。利用转移矩阵将源点云进行变换后，判断源点云与目标点云匹配对之间的距离，统计小于给定阈值的正确匹配对(inlier)个数。在所有实验中寻找 inlier 个数最多的一次抽样，输出相应的转移矩阵与点对应集合。

利用 RANSAC 算法，可以不必像 ICP 算法中依赖于两个点云的初始转移矩阵的估计。然而，由于点云中部分特征的相似重叠，同时可能存在较多的 outlier，通过这一算法得到的转移矩阵并不完全精确，需要后续算法对于匹配结果进行进一步的优化。

3.3 基于深度学习的点云精配准

正如上一小节所述，尽管 RANSAC 算法能够减少对初始值依赖，但在性能上存在缺陷。不过 RANSAC 算法能够输出的两个点云的粗匹配点对与转移矩阵，这为后续的精细化提供了一个初始值。在本研究中使用 3DRegNet 网络排除粗匹配对中错误匹配对，并对转移矩阵进行精细化，本小节主要对该网络的结构与损失函数进行精细化。

3.3.1 模型结构

3DRegNet 的网络结构如图 3.4 所示，该网络由两个子模块组成，分别是分类模块(classificationblock)与配准模块(registrationblock)，前者的输入是若干对含有噪声的点匹配对，该模块对每个点对是否是 inlier 进行分类，并输出置信权重；后者能够直接输出从源点云到目标点之间的 6DoF 参数，即旋转与平移的 6 自由度参数。

图 3.4 3DRegNet 网络结构图

分类模块结构如图 3.5 所示，个 3D 匹配点对首先通过一个全连接层，并使用 ReLU 函数作为激活函数，个不同的点对共享相同的权重，输出 × 128 维的向量。然后该输出通过个连续的 ResNet 模块[58]，最后将输出通过以 ReLU 和 tanh 函数

作为激活函数的全连接层，得到个匹配点对的置信权重 1, , 。

配准模块如图 3.6 所示，分类模块的每一层输出通过 pooling 提取有意义的特征，维度为 128×1，总共有 + 1 个 pooling 层，，其中提一个 pooling 层对应第一个 ResNet 模块的输入，最后一个 pooling 层对应最后一个 ResNet 模块的输出。将

图 3.5 3DRegNet 网络分类模块

所有特征拼接并规范化后得到维度为( + 1) × 128 的特征，合并特征通过一个 8 通道的卷积层，卷积核大小为 3 × 3，水平步长为 1，竖直步长为 2。卷积层的输出通过两个全连接层，最终得到 + 3 维的向量，其中前个参数(1, 2, , )为旋转信息，后 3 个参数(1, 2, 3) 为平移信息。

图 3.6 3DRegNet 网络配准模块

3.3.2 损失函数

3DRegNet 的损失函数由两部分组成，分别是来自于分类的损失和来自配准的损失。分类损失使用交叉熵来惩罚错误的匹配分类

其中是配准模块在计算时 ReLU 和 tanh 函数之前的输出，指 sigmoid 激活函数。(., .) 是交叉熵函数，是分类的 ground-truth，取值为 0 或 1，代表第个匹配对是 inlier 还是 outlier，是平衡不同对匹配点交叉熵损失的权重。配准的损失由源点云经过变换后和目标点云对应点之间的距离得到。

其中(., .) 为距离度量函数，和是由网络输出的参数计算得到的转移矩阵与平移向量。

网络的总损失为

其中和是平衡分类损失与配准损失的超参数。

3.4 实验与结果

3.4.1 数据采集

利用实验室已有的 Leica 激光三维扫描仪设备，本实验首先在清华二校门和客厅分别进行了点云数据采集，分别代表了在三维重建中的室外大场景环境与室内环境，其中客厅场景是封闭的，面积约为 30m2 左右。在每个场景中选择了三个不同的位置，将扫描仪设为中等精度进行扫描，其扫描点云详情如表 3.1 所示。

表 3.1 扫描点云数据

扫描编号	场景	点数量	文件大小
1	室内	11686747	568.78MB
2	室内	12319474	599.61MB
3	室内	10272879	500.20MB
4	室外	6382625	309.88MB
5	室外	6284718	308.37MB
6	室外	9435253	460.13MB

图 3.7 展示了以上两个场景的点云数据。其中图 3.7(a) 和图 3.7(b) 分别是测站 1

和测站 4 的数据预览图。

(a)室内环境

(b)室外环境

图 3.7 实验数据预览

3.4.2 全局点云配准实验

在这一小节中，本文基于上述 coarse-to-fine 的点云配准框架，试图采用增量式重建的方法对多点云进行融合，生成场景的三维模型，即首先从所有点云数据中选择两个点云作为源点云和目标点云进行配准，然后将配准后的点云看做目标点云，再选择一帧新的点云数据作为源点云，估计转移矩阵，重复进行上述操作直至所有点云之间都被有效地配准，完成场景重建。

以室外场景重建为例，接下来介绍在本次算法实验中一些参数的选择。在 RANSAC 粗配准中，本文采用了两种剪枝算法，来提早对错误匹配进行过滤，第一个是检查两个点云的距离是否小于 0.2，第二是在源点云和目标点云随机选择两对对应点连线，检查‖edge ‖ > 0.9 ‖edge‖ 和‖edge‖ > 0.9 ‖edge ‖是否成立。根据 Choi et al.[59] 提供的经验，本文将 RANSAC 算法中的最大迭代次数和最大验证次数设为 4000000 和 500。对于 3DRegNet 网络，本研究利用了作者提供的预训练模型，该模型在 ICL-NUIM 数据集和 SUN3D 数据集[60] 上进行训练，前者包括了 4 个不同的场景共 25000 个不同的匹配点对，后者包括了 13 个场景共 3700 个匹配点对。在 3DRegNet 网络中，设置 = 8，损失函数中分类和配准的平衡系数分别为 = 0.5, = 103。

首先选择扫描 4 和扫描 5 的点云进行配准。这两个点云的初始位置如图 3.8(a) 所示，分别在二校门南侧的左右两边扫描得到，以大小为 0.3 的体素对点云进行降采样后得到的结果如图 3.8(b)，为了更直观的显示，这里将原始扫描中的颜色信息去除，并将测站 4 设为红色，测站 5 设为蓝色；下一步对点云进行法线估计与 FPFH 特征值计算，点云法线信息如图 3.8© 所示，箭头方向为该点处的法线向量；经过 RANSAC 粗配准后，结果如图 3.8(d) 所示，两个点云被较好地对齐，但部分地方的效果依旧不够理想，同时粗配准也能够对两个点云中的点进行粗配对，配对结果如图 3.8(e) 所示，这里随机选择了 500 对匹配对，并用绿线相连接；将匹配对输入 3DRegNet 网络，得到最终转移矩阵的估计值，将此转移矩阵作用在降采样之前的点云上，其配准结果如图 3.8(f) 所示。

利用上述双点云融合的过程对多个点云数据进行融合，得到结果如图 3.9 所示，其中图 3.9(a) 是二校门场景的重建结果，图 3.9(b) 是客厅场景的重建结果，两个场景中的三个点云分别用红色、绿色和蓝色进行了标记。从主观上来看，不同位置扫描得到的点云融合效果较好。

3.4.3 算法评价

不同点位测量的局部点云结果的配准结果能够直观地评判，但缺少客观的评价指标。因此，为了能准确评估点云配准的算法性能，为估计结果提供 ground-truth 参照，本文采用人工变换的方式，对同一点位测量的点云中进行采样和处理，并提供随机的旋转和平移，得到目标点云，通过比较若干种点云配准算法的结果与 ground-truth 之间的差异，来说明本文介绍的算法的有效性。

以表 3.1 中测站 4 的数据作为对象，本实验采用 4 种不同的方式来生成源点云与目标点云。在第一组实验中，源点云由从扫描点云中直接随机采样的 20000 个点构成，并给予随机旋转和平移得到目标点云，即源点云与目标点云中的每个点都存在着一一对应的关系；在第二组实验中，源点云和目标点云为扫描点云中分

(a)初始点云 (b)降采样点云

©法线估计 (d)粗配准

(e)点对估计 (f)精配准结果

图 3.8 双点云融合过程及结果

别采样的 20000 个点，并给予目标点云随机旋转和平移；在第三组实验中，先按照第一组的方式生成源点云与目标点云，然后对目标点云中的每个点的每个维度坐标，加入了均值为 0，方差为 0.01 的高斯噪声，模拟数据采样中存在的噪声情况；在第四组实验中，整体点云被分割成三个相连的子空间 = {1, 2, 3}，源点云从{1, 2} 中采样 20000 个点，目标点云从{2, 3} 中采样 20000 个点，并给予随机旋转和平移，该组实验模拟两个点云之间只存在部分匹配(partial-to-partial)

(a)二校门场景

(b)客厅场景

图 3.9 多点云融合结果

的情况。

在本实验中，采用了三种点云配准方法，分别为经典的 ICP 算法（初始旋转矩阵为单位阵，平移向量为零向量）、FPFH+RANSAC（即仅进行粗配准），以及本文提到的 coarse-to-fine 的配准方法。所有点云处理前先进行了归一化操作，在随机变换步骤中，每个轴旋转角度, , 从[45, 45] 中均匀随机采样得到，每个轴向平移距离值 1, 2, 3 从[0.5, 0.5] 中均匀随机采样得到，旋转矩阵可以表
示为

平移矩阵为

配准结果如图 3.10 所示，分别展示了四组实验下三种不同方法的结果及其与 Groundtruth 的对比，为了更好地展示匹配效果，此处将原始点云中的彩色数据替换为了统一的颜色，源点云被标记为红色，目标点云被标记为蓝色。图 3.10(a) 对应了第一组两个完全相同点云的情况，此时 ICP 算法陷入了局部最优，没有很好地配准两个点云，而 RANSAC 算法能够较好粗对齐二者，并在通过 3DRegNet 网络后达到几乎完美的匹配，需要注意的是，在这组实验中由于配准后两个点云的点会完全重合，因此在 RANSAC+3DRegNet 和 Groundtruth 图像中只能看到红色点云的部分，这一结果也能证明本文算法的有效性。图 3.10(b) 和图 3.10© 分别对应重复采样的点云和含噪声的点云的情况，从直观上来看，ICP 算法和本文算法都能够很好地完成配准，并且两者看不出差异。图 3.10(d) 对应这部分重合点云配准实验，在这一组实验中，ICP 算法无法找到最优解，因为源点云中大多数点无法在目标点云中找到对应，而 RANSAC 算法的对齐效果虽然不够完美，但也给 3DRegNet 网络提供了一个较好的初值，而精配准后的结果效果直观上较好，与 Groundtruth 较为接近。

对于点云配准精度的评价，主要是计算估计的旋转矩阵和平移向量与真实旋转矩阵和平移向量之间的距离。目前最常用的度量是平均相对角度误差(MeanRelativeAngularError,MRAE)和平均相对平移误差(MeanRelativeTranslationEr-ror,MRTE)，除此之外还有均方误差 MSE，绝对平均误差 MAE 等。本文选用 MRAE 和 MRTE 来评价本文算法，两者的计算如下

其中和表示第次配准实验的预测旋转矩阵和平移向量，和表示第次实验的真实旋转矩阵和向量。MRAE 的单位是角度，MRTE 的单位是长度，根据表达式 3.12 和 3.13 可得，若预测的和与真实值越接近，则两个评价指标越接近于 0，表示配准的效果越好。

本文在室内和室外的两个扫描点云上分别对四组实验进行了 10 次重复实验，每次采样的点和转移矩阵均为随机生成，分别计算这 10 次实验的 MRAE 和 MRTE 指标的平均值，结果分别如表 3.2 和表 3.3 所示。

表 3.2 室内扫描点云配准结果评价

	ICP	MRAERANSAC	Ours	ICP	MRTERANSAC	Ours
完全相同点云	26.1960	9.9024	0.0000	0.3867	0.0337	0.0000
重复采样点云	24.8931	6.5319	0.0164	0.3083	0.0180	0.0001
含噪声点云	20.2065	21.8961	14.7626	0.3061	0.0771	0.0410
部分重叠点云	41.0603	37.7057	31.4172	0.3169	0.1382	0.1132

表 3.3 室外扫描点云配准结果评价

	ICP	MRAERANSAC	Ours	ICP	MRTERANSAC	Ours
完全相同点云	54.6477	7.3719	0.0000	0.5774	0.01359	0.0000
重复采样点云	67.8208	7.8853	0.1622	0.5998	0.0113	0.0002
含噪声点云	33.0697	49.0816	36.0937	0.3102	0.0376	0.0124
部分重叠点云	52.7475	14.6118	0.1655	0.3968	0.0105	0.0001

从表 3.3 中可得，由于给予 ICP 算法的初值是单位矩阵，因此算法常常会收敛到错误的局部极小值，因此角度和位移误差都很大，而经过 RANSAC 能够很好的估计出平移向量 t，因此首先能够使 MRTE 减小，进一步通过 3DRegNet 网络的精配准，使误差进一步缩小。另一方面，从四个实验组来看，不管采用何种方法，其匹配误差满足：完全相同的点云 < 重复采样点云 < 含噪声点云 < 部分重叠点云，而后两种情况更加接近真实数据采集的情况，同时本文的算法在这两组实验中的表现较好，因此在处理真实扫描数据中能够获得比传统方法更好的配准效果。

3.5 本章小结

本章结合传统点云配准方法和深度学习方法，提出了一种新的点云配准流程，该方法能够不依赖于点云的初始转移矩阵的估计，首先从点云中估计粗匹配对，然后再利用 3DRegNet 对匹配结果进行精细化调整。

同时，本章通过对在室内和室外分别采集的数据进行了实验，利用上述点云配准方法完成了场景构建并对该算法性能进行了评价。

(a)完全相同点云

(b)重复采样点云

©含噪声点云

(d)部分重合点云

图 3.10 室内扫描点云配准实验结果

第 4 章基于 2D-3D 的图像-激光跨模态匹配定位

4.1 引言

高精度离线地图的构建是复杂的，需要高精度的仪器设备对场景进行数据采集，并对点云进行配准，利用上一章的算法，能够由粗到精地对点云进行很好的对齐，得到的稠密点云地图，该地图能够包含更多的特征，为在场景中的智能体的定位提供更多的参照。

本章旨在解决通过视觉方式实现在先验场景中的定位问题，这需要建立其 2D 与 3D 模态之间的匹配。但是，如图 4.1 所示，平面图像之间与点云之间的特征能够分别建立统一的关系，实现查找和匹配，但图像与点云的点由于在模态和外观上存在巨大的差异，无法构建一个统一的描述子，因此难以进行直接匹配[61]。

图 4.1 2D 图像特征匹配与 3D 点云特征匹配

本章针对这一挑战提出了一个流程，将难以直接匹配的 2D-3D 问题转化为 2D-2D 匹配问题。该算法的流程如图 4.2 所示，离线地图中储存了每个点的 xyzrgb 信息，可以先通过采用虚拟相机成像的方法，将场景的点投影到空间中的某一有限平面上，模拟在场景中的某一位置以若干视角拍摄的照片，这一步完成的是从 3D 点云到 2D 图像的转换。利用全景相机可以拥有到更广阔的视野，并且相比于激光雷达可以获取环境中的颜色信息，因此在本算法中被用作定位设备。但全景相机需要进行畸变校正，故首先对拍摄的全景图像进行切分校正，然后在两个图像对之间，利用基于深度学习的 SOS-Net[62]进行鲁棒的特征点匹配，最后利用对极几何的方法求得 2D 图片与场景中某一已知位置之间的转移矩阵，从而实现定位的目的。

图 4.2 基于 2D-3D 匹配的高精度点云场景定位算法流程图

4.2 点云-平面映射

4.2.1 虚拟视点图像生成

相机能够将从真实三维世界中获取点的颜色信息，并将其投影到胶片上，将三维世界的点转变为平面上点的过程被称为相机模型。本文采用了最简单针孔相机模型，将点云图像中的点投影到给定虚拟视点和虚拟成像平面上。

图 4.3 解释了该模型的原理图，是相机的光心，也是本算法中的虚拟视点，以点为中心构建三维直角坐标系，该坐标系成为相机坐标系，真实世界中的点透过光心落在光心后的物理成像平面′ ′ ′ 上，该成像平面与平面平行，投影点为 ′，将光心到成像平面的垂直距离记作焦距，设点坐标为[, , ] ， ′ 坐标为[′, ′, ′] ，则根据相似三角形的性质，有

其中负号代表相机成像是道理的。为了实验与分析的方便，一般将成像平面至于

图 4.3 针孔相机成像原理图

相机前方，则 4.1 可变为

整理得

为了获得点在图像中的像素，还需要做一次坐标转换，将成像平面坐标系转换为像素坐标系。这两者之间相差一个尺度和位移变换，，假设点的像素坐标为[, ] ，则其数学表达式为

其中, 表示轴和轴上的缩放系数，, 代表成像中心在像素坐标系下的坐标。将式 4.3 带入上式，并写成矩阵的形式可得

其中是真实世界的像素坐标，定义为相机的内参矩阵。

通常来讲，按照 4.5 计算出来的,值往往是小数，因此可以采用插值法对每个点的 RGB 通道进行插补，常用的插值方法有最近邻插补法、基于三角形的线性插补法、以及基于三角形的三次插补法。这几种方法的效果依次递增，但同样计算开销也较大。同时需要注意的是，通过后两种方法得到的颜色通道值有可能超出 0-255 的范围，因此需要进行截断。

4.2.2 实验结果

本章利用表 3.1 中测站 4 的数据进行了实验，图像生成算法的原理示意图如图 4.4 所示。

图 4.4 虚拟成像原理图

设置虚拟相机的焦距 = 1，成像平面为 = 1 的平面，同时为了固定成像平面大小，将水平视角设为 90，垂直视角设为 60，图像大小 960 × 640，, , 轴的轴旋转角, , 分别为 90, 90, 0，采用线性插值的方法对像素点插值，结果如图 4.5 所示。由于这里采用了室外点云进行实验，部分区域（如天空）存在大面积点缺失现象，直接采用插值的结果会与真实成像结果差距较大，因此为了避免失真，在算法最后对所得到的图像进行掩模处理，将大面积无投影的区域颜色置为纯黑色。

从图中中可以得到，从点云中恢复的平面图像能够较好的模拟真实相机的拍摄效果，这可以为下一步图像之间的匹配提供了较好的输入。

图 4.5 点云-平面映射实验结果

4.3 全景畸变校正

由于全景相机能够带来更广阔的视野，为后续特征匹配寻找关键点提供更多信息，因此在本研究中采用全景相机进行定位，但全景相机由于其自身的成像原理，会带来很大的畸变，因此在本节中主要介绍这种相机的成像原理，并对其进行畸变校正。

4.3.1 全景成像原理

与图 4.3 所示的针孔相机不同，全景相机的投影面不是一个平面，而是一个球面，如图 4.6(a)所示，首先建立相机坐标系，在相机坐标系下，全景相机将真实世界中的一点 = (, , ) 投影到单位球上 = ( , , )，即满足‖ ‖ = 1，根据相似三角形，有

转换为球坐标系，有

同样，从相机坐标系转换到像素坐标系需要进行尺度和平移变换，其变换形式与式 4.4 类似，如下

(a)相机坐标系 (b)球坐标系

图 4.6 全景相机成像原理图

4.3.2 畸变校正原理

如图 4.7 所示，在照片成像的过程中，由于相机本身的构造或者人为组装上的偏差，相机成像后往往会带来畸变，造成空间上的直线投影到成像平面后产生了弯曲，在知道畸变参数之后，往往可以对畸变图像进行校正恢复。

图 4.7 相机畸变示意图

与针孔相机带来的畸变不同的是，全景相机的畸变是本身成像平面的弯曲带来的，因此可以利用上一节中介绍的原理，对全景相机图像进行畸变校正，根据弯曲方向的不同可以将畸变分为桶形失真和枕形失真两类。

如图 4.8 所示，该算法使用了一个新的成像平面 = 1，并确定了平面图像的大小 × ，因此类似于在点云中的虚拟成像原理（见第 4.2 节），可以将全景图的每个像素看做三维空间中单位球上的若干个点，从点出发连接这些点的射线与 = 1 平面的交点即为平面图投影点，同样这些点在新成像平面上的坐标值大多是小数，因此最后需要通过插值法得到某一视角下的畸变校正结果。

图 4.8 全景图像畸变校正示意图

4.3.3 实验结果

本节实验选择理光全景相机 RICHOTHETAV 作为拍摄设备，如图 4.9 所示，该设备通过前后两个鱼眼镜头拍摄的图像来合成全景图，拥有 ISO3200 的图像感光度和 6400 的视频感光度，以及 1/2500 秒的快门速度，并且支持 4K60fps 高清视频录制。

图 4.9 RICHOTHETAV 全景相机

使用该设备在二校门处拍摄一张全景图像，如图 4.10(a)，相机拍摄的全景视图固定水平视角为 90，垂直视角为 60，分别在水平转向角为 45,135,225,315 的位置生成校正图，其结果如图 4.10(b) 所示。

(a)二校门全景图像

(b)局部畸变校正结果

图 4.10 全景畸变校正实验数据及结果

4.4 基于 SOSNet 的图像特征提取与匹配

本章先通过点云合成的 2D 图像与全景图的 2D 图像之间进行特征点的匹配，然后进行相机位姿的估计。这一方法的主要问题是通过点云生成的视图与定位时的视图可能在光照条件、位置等方面存在着较大的变换，因此需要一个鲁棒的图像特征提取与匹配算法。

本文采用了 SOSNet 的方法，该网络将二阶相似性(SecondOrderSimilarity)纳入损失函数当中，在多个基准数据集上收获了较好的性能。

4.4.1 网络结构及损失函数

该网络的结构与 L2-Net[63]相同，如图 4.11 所示，它采用了一个全卷积的结构，降采样通过一个 stride 为 2 的卷积层实现。在每层卷积层之后连接了 BN 层，最后将局部响应归一化层(LRN)[64]作为输出层来生成描述符。该网络能够将输入的 32×32 的 patch 映射为 128 维的向量。

图 4.11 L2-Net 网络结构

与 L2-Net 不同的是，SOS-Net 在训练时的损失函数设计，不仅仅考虑了特征之间的一阶相似度(FOS)，也将二阶相似度(SOS)纳入其中，其中一阶相似度的计算如下

其中

式中和 + 分别表示两幅图像中匹配的 patch 特征，pos 衡量了正确的匹配对之间的欧氏距离，neg 表示了匹配对之间的距离。

对于两个匹配对和 +，首先定义两者的二阶相似性

基于 SOS 的正则项可以表示为所有 patch 与对应 patch 的二阶相似性平均值

总 loss 看做一阶相似性和二阶相似性的直接加和

4.4.2 特征提取与匹配结果

本实验分别从点云生成的平面图和通过全景相机生成的局部平面图中各选择一帧，首先用 BRISK 算法[65]对特征点进行提取，该算法能够保证特征提取的旋转不变性和尺度不变性，特征点提取结果如图 4.12，上半部分为点云生成的图像，下半部分为利用全景相机生成的图像，其中前者提取到的特征点数为 231 个，后者提取的特征点数为 230 个。

图 4.12 BRISK 算法特征点提取

在特征提取算法中，首先通过 SOSNet 生成两张图片中每个特征点的特征描述符，对于每个查询点，寻找其在另一张图中的前 2 个最佳匹配结果 1, 2，若‖ 1‖ < 0.88‖ 2‖，则该匹配被认为是好的匹配，和 1 被标记为匹配点。

利用上述算法对二校门的两张图片之间的特征进行匹配，其结果如图 4.13(a)，为了对比该算法的效果，本文同时采用了 ORB 算法进行了特征匹配，其结果如图 4.13(b)。可见，利用 SOSNet 网络能够得到 14 对特征匹配对，且匹配基本正确；而使用 ORB 算法虽然得到了 21 对特征匹配对，但基本均为误匹配，效果不如 SOSNet。

(a)SOSNet 特征匹配结果 (b)ORB 算法特征匹配结果图 4.13 特征匹配结果及对比

传统方法与基于学习的方法之所以在本研究中的特征提取任务上表现差异较大，主要是因为利用点云投影算法得到的平面图，与真实图像之间有所差距，同时两帧图像的位置差别也较大，因此常规的特征提取算法往往难以产生好的效果，而使用深度学习方法能够鲁棒地提取特征描述子，同时得益于图像二阶相似性在训练中的应用，使得相同匹配 patch 的特征描述符在欧式空间较为接近，因此产生了更好的匹配效果。

4.5 基于对极约束的定位算法

4.5.1 对极约束

在三维空间中，同一个点在两个不同相机的成像平面上的投影位置与这两个相机之间的运动和它们的内参之间存在着一个关系，这种关系被称为对极约束，因此，利用若干点对的对应关系，就能求取两个相机之间的运动。在上一节中，本文利用了 SOSNet，对在点云某一位置生成的图像和全景相机图像之间的特征点进行了鲁棒的匹配，因此可以将点云的位置看做参考坐标系，利用对极约束求取这两者之间的运动，从而得到全景相机在点云中的相对位置。

图 4.14 对极约束原理示意图

以图 4.14 为例，1 和 2 分别是空间中的两帧图像，我们希望求取这两者之间
的运动，即求取, 。空间中有一点，坐标为[, , ] ，根据针孔相机模型，
有

其中 1/可以看做 1 在归一化平面上的坐标，因此不妨令

得

上式等号两侧左乘 ^，符号表示与做外积，得到 ^2 = ^1，然后同时左乘 2 ，得

定义本质矩阵 = ^，，这是一个 3×3 的矩阵，因此本文算法分为以下两个步骤：

1.根据匹配的特征点，计算本质矩阵.这一步通常采用 8 点法[66-67]进行求解，将矩阵的各个元素用线性方程进行表示并求解。

2.根据本质矩阵，求取, 。这一步可以采用 SVD 分解的方法进行恢复[68]。

4.5.2 定位实验

1.单帧图像定位实验

本文利用图 4.13(a) 的特征匹配结果进行了单时刻的定位。由于不可避免地由于参数设置不能很好地处理不同场景的图像特征，SOSNet 的匹配结果中也有可能产生误匹配的现象，因此为了减小错误匹配对结果的影响，类似点云配准的操作，本文采用了 RANSAC 方法，随机的从匹配点中选择八对特征点进行本质矩阵的估计，并选择在未被选中的点中拟合效果最好的一次作为最终模型结果。

计算得到的本质矩阵为

从本质矩阵中恢复出从点云平面图到全景平面图之间的相机运动为

由此根据，便得到了在点云坐标系下的相机坐标。

2.多帧连续图像定位实验

与单帧图像类似，本文在基于连续视频的定位上进行了实验，这次实验场景选在了室内，离线地图是图 3.9(b) 中合并的地图，对于其中的三个子点云，分别以它们的中心为虚拟观测，选择了 4 个角度生成平面合成图，因此总共得到了 12 张

离线子地图，以及 12 个它们对应的位置和视角信息。一个连续视频流的定位问题可以分解为一下两个步骤，第一是能够在场景中不依赖先验条件的情况下初始化定位，第二是根据上一帧的定位信息以及当前数据更新当前时刻的位置信息。本文按照以下算法处理这两个问题：

初始化定位选定水平视场角和垂直视场角，将全景图沿某一方向进行投

影，分别在 12 张离线地图图像中搜索特征匹配，若某一张图片特征匹配数大于阈值，则以该图片对应的测站作为基准，估计其到全景相机的运动，得到初始化坐标；若所有离线子地图均无法产生足够的有效配对，则表示全景图的投影方向可能特征不够明显，更换投影方向重复上述操作，直至产生足够多有效配对为止，最终得到初始的位置。

基于先验信息的位置更新对于全景相机，保持与上一帧的投影方向不变；

由于知道了相机在上一帧中对于离线地图的旋转角和平移向量，因此选择距离上一帧位置最近的测站作为基准，在其中的 4 张离线地图图像中选择视角与全景投影视角相差最小的合成图像进行配对，从而更新相机的位置和角度信息；若无法产生有效配对，则更换全景投影方向，并依次与最近测站中的 4 张平面图进行配对，最终得到当前时刻相机的位置和旋转角。

利用以上算法，以 544×960 的分辨率、30fps 的帧率在室内场景中拍摄一段

20 秒左右的全景视频，同时每隔 5 帧更新一次位置信息，因此相当于对 120 张时序图像进行了定位，其结果如图 4.15 所示，其中黄色线条为相机的运动轨迹，为了更好地看清建图及定位效果，对屋顶部分进行了去除。

4.6 本章小结

本章解决了基于离线点云地图的视觉定位的问题。在整体框架上来看，本章将 3D-2D 匹配问题转换成了 2D-2D 匹配问题，在离线地图中生成局部视点，合成某一视角的平面图像，并利用基于图像二阶相似性的 SOSNet 网络对离线图像与在线图像之间进行特征匹配，最后通过对极约束求得两者的相对位置，即获得相机的六自由度。

同时，本章也在室外和室内分别进行了单帧图像的定位与图像序列的定位实验，实验结果均证明了本文提到的定位方法的可行性。

(a)俯视图

(b)侧视图

图 4.15 室内场景定位结果图

♻️ 资源

大小： 78.9MB
➡️ 资源下载：https://download.csdn.net/download/s1t16/87377748