【计算机视觉】基于三维重建和点云处理的扫地机器人寻路

[摘要]

扫地机器人的使用已经越发普及，其中应用到了三维重建的知识。本项目旨在设计由一 定数量的图像根据算法完成三维模型的建立，并利用三维数据最终得到扫地机器人的行驶路 线， 完成打扫机器人成功寻路的任务 。本项目采用的方法是 SFM-MVS 、Colmap 、Kinect 三 种建模方法进行建模， 分别由组内不同成员完成， 经过亲自采集一定数量的图像集， 利用 SFM-MVS 算法获得对应的三维模型进行 2D 降维处理， 并利用该结果设计路径规划算法， 从而完成任务。此次项目三种算法之间的结果对比，为三维重建建立了更加客观的理论基础 和算法基础， 同时也拓宽了三维重建应用领域， 将三维重建真正应用到实践中。

关键词： 三维重建  SFM MVS Colmap Kinect  点云  降维  2D  路径规划

1.  项目背景

(1) 三维重建

三维重建是指对三维物体建立适合计算机表示和处理的数学模型，是在计算 机环境下对其进行处理、操作和分析其性质的基础，也是在计算机中建立表达客 观世界的虚拟现实的关键技术。

在计算机视觉中,三维重建是指根据单视图或者多视图的图像重建三维信息 的过程 。由于单视图的信息不完全， 因此三维重建需要利用经验知识。而多视图 的三维重建（类似人的双目定位） 相对比较容易,其方法是先对摄像机进行标定, 即计算出摄像机的图象坐标系与世界坐标系的关系。然后利用多个二维图象中的

信息重建出三维信息。

本次实验通过 SFM-MVS 、Colmap 、Kinect 三种不同的算法将多个二维图像信息重建出三维信息。

由于我主要负责 SFM-MVS 算法的设计，所以对此算法流程的描述会重点详细描述。

(2) 降维

数据降维是指采用某种映射方法，降低随机变量的数量。例如将数据点从高维空间映射到低维空间中， 从而实现维度减少。

本次实验采用的是将三维数据降维到二维数据。

(3) 路径规划

随着机器人技术 、智能控制技术 、硬件传感器的发展， 机器人在工业生产 、  军事国防以及日常生活等领域得到了广泛的应用。而作为机器人行业的重要研究    领域之一，移动机器人行业近年来也到了迅速的发展。移动机器人中的路径规划    便是重要的研究方向。移动机器人的路径规划方法主要分为传统的路径规划算法、 基于采样的路径规划算法 、智能仿生算法 。传统的路径规划算法主要有 A 算法 、 Dijkstra 算法、D 算法、人工势场法，基于采样的路径规划算法有 PRM 算法、RRT    算法， 智能仿生路径规划算法有神经网络算法 、蚁群算法 、遗传算法等。

本次实验由于时间原因，暂时设计一个简单的贪心算法来完成算法设计，并 且将其应用到降维后的数据中。

(4) SFM

SFM（从运动恢复结构， Structure From Motion）是从一系列包含视觉运动信 息的多幅二维图像序列中估计三维结构的技术。通过相机的移动来确定目标的空

间和几何关系， 是三维重建的一种常见方法。

(5) MVS

MVS（多视点立体视觉，Multi-view stereo）能够单独从图像中构造出高度细 节化的 3D 模型， 采集一个庞大的图像数据集， 用其来构建出一个用来解析图像 的 3D 几何模型。

(6) 主要问题和需求

三维重建是计算机视觉领域比较新兴潮流的领域，与以往计算机视觉领域中 分类问题不同，三维重建模型很难有具体的评价标准。分类问题可以通过模型对 测试集的结果进行正确率的计算，但是三维重建的结果是无法通过简单的计算得 到准确率的， 所以对于模型的评判结果比较主观 。所以得到三维模型固然关键， 但是随之而来的最关键问题是能够有一个标准来对三维建模模型的优劣进行判断和分析。

扫地机器人的应用是一个我们感兴趣的领域，同时扫地机器人如何做到识别 空间信息并且做出合理的路径规划是我们非常好奇的， 我们对三维重建感兴趣，

同时又好奇扫地机器人的核心算法原理，所以我们一致决定选择这个方向来进行 研究。

(7) 解决方法

本小组考虑到三维建模的评价体系欠缺量化标准，同时又想把三维重建真正   作用到实际生活应用中，我们想到了扫地机器人这个与生活息息相关的实践项目。

扫地机器人需要能够识别障碍物，然后进行路径规划，再把垃圾等放入身体 中，识别障碍物就是需要扫地机器人能够由图像建立三维模型，并且三维模型经 过一定程度的降维使扫地机器人能够计算出障碍物的具体位置。路径规划是一种

算法， 可以计算出扫地机器人可以避开障碍物清扫垃圾的最佳路线。

基于此， 我们此次项目通过完成对 SFM-MVS 、Colmap 、Kinect 的建模， 得 到一个三维模型， 并对得到的三维模型进行降维处理， 得到一个平面上的点集， 在这个过程中，设置阈值把临界值之内的点在降维后归为障碍物类。然后开始执 行路径规划算法， 就完成了扫地机器人的必要算法部分。

(8) 意义和价值

本项目完成了扫地机器人的初步版本，不仅完成了三维模型的建立过程，也 和算法相结合，解决了复杂的生活实际应用问题。通过此次项目的完成，也完成 了在生活照片上对于 SFM-MVS 、Colmap 、Kinect 三种算法的优劣性的对比 。为 三维重建建立了理论基础 、算法基础 、应用基础。

2.  相关工作

目前国内外对基于图像的三维重建技术这一热点技术有很多的研究，已经有  很多成果和进展，主要是对特征检测、特征匹配、摄像机标定几个部分进行研究。 有很多的基于图像的三维重建软件 。基于图像的三维重建技术具有快速 、简便、 逼真的优点，能较好的实现现实事物的虚拟化，该方法尤其适合那些难以用 CAD  的方法建立真实感模型的自然环境， 以及需要真实重现环境原有风貌的应用。

(1) 国外研究现状

1     Paul E.Debevec——参数几何体表示初始模型

2     Steven M.Seitz——颜色不变量 、顺序可见性规则重建场景模型 3      Roberto cipolla——三维重建系统 PhotoBuilder

(2) 国内研究现状

1     北京交通大学的袁保宗提出了，由真实世界到计算机虚拟世界的转换问 题。

2     浙江大学的刘刚设计了，一个能绘制出几何模型和表面纹理的真实场景 交互建模系统。

3     中科院自动化研究所，开发的 CVSuite，能利用立体视觉进行三维重建。

4     上海交大马利庄提出了一种基于构建 Visual Hull，求取物体形状及表面 反射属性的方法。

(3) 相关技术分析 、评价和比较

SFM： MVS 算法的性能只取决于输入图像的质量和摄像机参数， 所以 MVS 的成 功很大程度上归功于底层的用来计算摄像机参数的 SFM 算法 。SFM 算法以一组 图像作为输入，生成两个信息：每幅图像的摄像机参数和图像中可见的一组三维 点， 这些点通常被编码为轨迹 。轨迹被定义为重建的 3D 点的 3D 坐标和输入图 像子集中对应的 2D 坐标的列表。

MVS：稠密重建是假设相机参数已知的情况下，寻找空间中具有光度一致性的点， 对场景进行立体匹配的过程。

Colmap：COLMAP 是一种通用的运动结构  (SfM)  和多视图立体  (MVS)  管道，具 有图形和命令行界面 。它为有序和无序图像集合的重建提供了广泛的功能。

Kinect： 首先用 Kinect 设备采集点云， 然后对点云进行地面分割提取主体， 过 滤掉噪点， 用 ICP 算法将不同视角的点云进行配准， 最后用滚球法进行表面重 建。

这四种算法的具体应用效果， 将在实验结果中展示并详细剖析。

3.  研究方法（技术路线）

(1) 开发环境

PyCharm 、Jupyter notebook 、python3.7 、MVStudio

(2) 关键技术

SFM

1     特征提取

一般采用 SIFT 算子, 因其具有尺度和旋转不变性。

2      匹配和建立 track， 图像对两两匹配， 一般采用欧式距离。 

有两种方法：

1)   粗暴匹配： 对所有特征点都穷举计算距离。

2)   邻近搜索：建立 KD 树，缩小搜索范围，能提高效率，但也有可能不

是最优， 所以邻域取值是关键， 越大越准确， 越大计算量越大。

当距离小于一定阈值的时候就认为匹配成功,但是误匹配也比较多,需要采取多种手段剔除:

如果最近距离与次近距离的比值大于某个阈值， 应该剔除。

对匹配点采用采样一致性算法 RANSC 八点法计算基础矩阵， 剔除不满足基 础矩阵的匹配对。

当匹配关系建立后， 需要生成 track 列表， 指同名点的相片集合， 比如第一  幅图的 13 号点和第二幅的 14 号点及第五幅的 115 号点是同名点，则(1,13)、（2,14)、 (5,115）是属于一个 track，据此可以生成一个 track 集合， 同时生成 track 的时候  也需要剔除无用匹配：

如果一个 track 包含同一幅图多次， 则应该剔除， 这是因为同一幅图的多个 特征点都匹配了同一个点， 则匹配关系肯定是错误的。

如果 track 太少，应该剔除，一般取 2，是指只有两幅图有同一个点，三维重 建的信息过少， 容易产生误差。

3     找初始化像对

目的是找到相机基线最大的像对,采用 RANSC 算法四点法计算单应矩阵，满 足单应矩阵的匹配点称为内点，不满足单应矩阵的称为外点，根据单应矩阵公式 可知当 T 越小时， 内点占比越高， 也就是低视差现象越明显。

因此找到一个内点占比最小的像对就是初始化像对，当然它前提必须满足可 重建,这个可以通过匹配点个数保证。

4     初始化像对的相对定向

根据 RANSC 八点法计算本征矩阵，可通过对本征矩阵 SVD 分解得到第二个 图像的 R 、T， 在这一步需要进行畸变校正， 然后根据 R 、T 和矫正后的像点坐 标三角计算出三维点，这里用到的方法是直接线性变换DLT，可以理解为测绘中 的前方交会。

5     加入更多图像

根据第四步生成的三维点和第三副图与前两图的 track 关系， 可以反算第三 副图的 R 、T， 然后继续三角化计算出更多的三维点， 采用的同样是 DLT， 这样 反复重复第 5 步， 最后就会把所有像片的 POSE（ R、T）和三维点，这就是稀疏 重建 SFM 的成果了。

6      从④开始需要进行光束法平差 Bundle Adjustment

这是一个非线性优化的过程，目的是使重建误差降低到最小，通过调整 POSE 和三维点使反向投影差最小， 如果相机没有标定,还应该将焦距也参与平差。

Bundle Adjustment 是一个迭代的过程，在一次迭代过后，将所有三维点反向 投影到各自相片的像素坐标并分别与初始坐标比对，如果大于某个阈值，则应将 其从 track 中去掉， 如果 track 中已经小于 2 个了， 则整个track 也去掉， 一直优 化到没有点可去为止。

MVS

1      稀疏 3D 点云重建(SFM)

检测输入的每帧图像特征点，并进行特征点匹配，通过匹配的特征点恢复相 机的内外参数（相机内参：焦距、径向畸变系数，相机外参：相机的位姿 R，t）， 重建特征点的空间 3D 坐标。

2     全局视角图像序列选取

从输入图像中选一帧作为参考图像。通过一些指标在输入图像中筛选出符合 要求的图像。这里我们希望筛选出来的图像和参考图像，它们相邻图像视场之间 的拍摄视场角合适（相邻图像视场角太小会导致基线短，重建误差大，视场角太 大则两张图差异太大也难以重建成功）。选取方法详细介绍见本文第二部分算法 原理。

3     种子点选取

从获得的 3D 点云中，筛选出那些可以投影到参考图像上的 3D 点，并且该点

能投影到至少 1 帧其他全局视场图像（有些点按照投影模型无法投影到参考图或 其他图的成像面上） 。

4     种子点深度与法向量信息恢复

将 3D 点投影到参考图像上， 以 3D 点到参考图像相机坐标系原点的距离为投影 点像素初始深度， 3D 点到相机坐标系原点的方向为初始法向量。

上面获得了像素点的深度与法向量的初始值，我们需要对这些像素点的深度 和法向量进行优化。以这些投影像素点为中心建立正方形模板，将每个模板里的 所有像素投影到空间 3D 坐标中， 以一个模板为例， 再次把一个模板里的 3D 点 投影到其他视角的图像上获得投影的像素点，优化像素点的深度与法向量信息使 得在参考图像上的模板与在其他视图上的模板投影差异尽可能小。详细优化算法

见本文第二部分算法原理

建立模板其实为了衡量参考图像上的这个像素点与其他图像上像素点的相 似性，仅凭一个像素点难以很好的表征，所以才引入了模板来表征中心像素点区 域的特征，而参考图像与其他图像的模板特征的差异是优化深度和法向量信息的 关键。

5     其余像素点深度与法向量信息恢复

计算种子点优化后结果的置信度，参考下文基本概念光度一致性——衡量两 个模板的相似程度，按照置信度高低建立队列，对队列中的点进行优化，并将这 些点的周围点加入队列中。

Colmap

1     数据采集

手机或者相机绕物体拍一周， 每张的角度不要超过 30 °。

2     数据组织

将多角度拍摄的图片组织为 colmap 的工程格式。

3     提取图像特征

从图像中提取到特征值。

4     特征点匹配

输出： 提取到的特征点也存放到数据库中。

5      稀疏重建(SfM）

使用 SFM 进行稀疏重建。SFM，(Structure From Motion，从运动中恢复结构)， 是一种从一组不同视角下拍摄的无序或有序影像中恢复场景三维结构和相机姿

态的技术。

input：  一组图片。

output： 场景粗糙的 3D 形状（稀疏重建）， 还有每张图片对应的相机参数。

6     深度图估计

深度图估计， 目的是恢复参考影像的深度信息。深度估计结束后， 可以得到 “photometric ”和“geometric ”下的深度图和法向量图 。在深度图估计之前要进 行图像去畸变操作。

7      稠密重建（ MVS）

使用 MVS 进行稠密重建 。MVS 即多视图立体几何， 目的是在相机位姿已知 的前提下，逐像素的计算图像中每一个像素点对应的三维点，得到场景物体表面 密集的三维点云。

输入： 多视角图像 、相机位姿。

输出： 稠密点云。

8     融合

9     稀疏重建结果可视化

10    深度图 、法向图可视化

11    稠密重建结果可视化

Kinect

1     深度坐标变换

对于每个像素启用一只 CUDA 线程，给定一个 Kinect 红外摄像头的内部校准 矩阵 K， 使用如下公式

/tertupui(u)=/tectupDi(u)/tectupk-1[u,1]

2     计算坐标变换

相应的顶点法线则直接取其右 、下邻向量的外积， 并进行归一化 。 时间 i 时的 6DOF 摄像机姿态使用一个刚性变换矩阵 T 表示，其中 T 包括了摄像机的旋转 矩阵 R 和平移矩阵 t 。变换后的顶点和法线通过 T 即可再变换至全局坐标系中。

3     摄像机跟踪

该部分的主要目的就是计算 6DOF 姿态 。核心的 ICP 即迭代最近点算法， 首 要 是 需 要 逐 帧 计 算 不 同 朝向 的 点 集 的 相 关 度 。 这 里 采 用 了 projective data association 方法计算相关度。

4     体集成

针对已配准的点云数据， 需要执行后续的融合处理。

5     光线投射渲染

采用光线投射渲染前步生成的隐式表面。

降维

在本实验中，降维是将三维模型信息降维到二维平面信息。对于噪声和阈值 的处理是降维的核心任务 。去噪声是为了减少无关信息对真实需求模型的影响， 阈值的选取是本次降维算法的灵魂，通过选取合适的阈值，从而选取有效信息区 间， 再进行 z 轴压缩， 从而完成降维处理。

路径规划

一般的连续域范围内路径规划问题，如机器人、飞行器等的动态路径规划问 题， 其一般步骤主要包括环境建模 、路径搜索 、路径平滑三个环节。

1     环境建模。环境建模是路径规划的重要环节， 目的是建立一个便于计算 机进行路径规划所使用的环境模型，即将实际的物理空间抽象成算法能够处理的 抽象空间， 实现相互间的映射。

2     路径搜索。路径搜索阶段是在环境模型的基础上应用相应算法寻找一条

行走路径， 使预定的性能函数获得最优值。

3     路径平滑。通过相应算法搜索出的路径并不一定是一条运动体可以行走

的可行路径， 需要作进一步处理与平滑才能使其成为一条实际可行的路径。

对于离散域范围内的路径规划问题，或者在环境建模或路径搜索前已经做好 路径可行性分析的问题， 路径平滑环节可以省去。

本次实验采用的路径规划算法主要是贪心算法， 后续会设计更复杂的算法。

4.  实验结果与分析

(1) 三维建模结果展示

                            SFM 算法结果                                                     MVS 算法结果

Colmap 算法结果

降维结果

Kinect 算法结果

路径规划

(2) 三种建模方法对比分析

SFM

数据集经过 SFM 模型得到三维模型 。实验结果中建筑物的点云比较稀疏，

呈稀疏分布，同时粗略表达出了一些纹理特征和一些简单的几何形状，符合预期 结果。

这是因为 SFM 先从检测图像中提取 2D 特征（SIFT or ORB） 表征 。这些图  像特征的表示为图像中的一个小区域。而 2D 特征的特点是可以可靠地表示高度 纹理区域或者粗糙的几何形状。但是这些场景特征需要在整个场景中唯一（比如 重复的墙纹理， 难以匹配） 。故而通过这些唯一的特征只能生成稀疏的 mesh 。 当图像之间找到很多匹配项时，可以计算出图像之间的 3D 变换矩阵从而有效地 给出两个相机之间的相对 3D 位置， 从而获得点云。

MVS

数据集经过 MVS 模型得到三维模型 。实验结果中建筑物的点云比较稠密， 呈稠密分布，已经能够表达出大部分纹理特征和一些复杂的几何形状，也符合预 期结果。

相比于 SFM，MVS 的表达细节更加丰富，更加接近实物情况。这是因为 MVS 算法用于细化通过 SFM 技术获得的网格， 从而产生所谓的密集重构 。此算法中 要求每个图像的相机参数都起作用， 这由 SFM 算法输出 。 由于它适用于更受约 束的问题（因为它们已经具有每个图像的摄像机参数，例如位置，旋转，焦点等）， 因此 MVS 将在 2D 特征未正确或无法正确检测的区域上计算 3D 顶点或匹配。

Colmap

Colmap 算法结果是通过输入多视角图像， 经过 colmap 算法得到的稀疏云， 并估计相机参数 。重建精度还是不错的， 并且整体操作上比较快。

经过查阅相关资料， 我们得知， COLMAP 改进了 SFM， 首先， 引入几何验 证策略，利用信息增强场景图，随后提高初始化和三角测量组件的稳健性。第二， 下一个最佳视图选择，最大化增量重建过程的稳健性和准确性。第三，一种稳健 的三角测量方法， 其以降低的计算成本产生比现有技术显着更完整的场景结构。 第四， 迭代 BA（光束法平差）， 重新三角化和异常值滤波策略， 通过减轻漂移

效应显着提高完整性和准确性。最后，通过冗余视图挖掘为密集照片集合提供更 高效的 BA 参数化。采用深度和法线信息的联合估计，使用光度和几何先验的像 素视图选择， 以及多视图几何一致性项同步细化基于图像的深度和法向融合。

由于 Colmap 算法的稀疏算法采用的是改进后的 SFM， 从而得到了场景中的 相机姿态和表示场景结构的稀疏点云 。 因而在一定程度上和单纯的 SFM 算法有 些相似， 通过结果观察发现， Colmap 算法不仅建立了 SFM 的表达出的基本轮廓 特征和图像， 也表达出了相机参数， 标识出了相机位置 。在建筑物表达方面， Colmap 要比一般的 SFM 算法更胜一筹。

Kinect

Kinect 算法结果得到的三维建模信息相比去其他几种算法更为丰富和具体， 但是也伴随着许多信息丢失甚至扭曲的现象发生， 即产生空洞。

Kinect 算法之所以发生图像扭曲的情况，是因为 Kinect 相机能够获取同一位 置的彩色和深度图像（距离图像），其中每个点像素的灰度值表示物体表面与相 机的实际距离。但是， 由于物体材质、镜面反射或区域遮挡等原因，Kinect 相机 在采集深度信息时容易在物体的边缘或内部丢失部分信息，即产生空洞（深度图 像中空洞表现为像素值为零的区域），深度图像中的空洞会给后期的图像处理带 来很多困难。因而也会间接影响我们扫地机器人后期对于降维以及路径规划的实际处理结果。

降维

以室内场景为数据集，我们通过算法得到了建筑物的三维模型，对三维模型 信息进行降噪、降维处理，得到室内地面二维平面图。通过控制降维时算法的阈 值，我们得到了比较合理的二维信息。通过结果观察可知，降维后的效果和真实 户型图很相似， 说明我们降维的效果还不错。

路径规划

得到二维信息之后， 问题就转化为在有障碍物的基础上设计路径规划算法， 使得扫地机器人能够合理扫过需要清扫的区域。

我们本次实验采用的是贪心算法。

5.  总结与展望

本次项目完成了 SFM-MVS 、Colmap 、Kinect 的三维重建方法， 分析了三种 不同建模方法对三维重建的影响，以及三种建模方法的优劣和适用情况。设计了 降维算法完成从三维到二维的压缩、设计了路径规划算法完成了扫地机器人的最

优路径选择， 至此扫地机器人的基本算法设计也就完成了。

通过本次实验， 我们了解并掌握了 SFM-MVS 、Colmap 、Kinect 等建模方法 的原理 、区别、优点、缺点、各自的使用范围等。通过代码实践，进一步对算法 的代码结构有了了解，对计算机视觉的三维重建任务有了更加深刻的理解。同时 我们的团结友爱、互相帮助，使得我们小组不仅在知识和成果方面收获满满，我 们也通过长时间的沟通了解收获了友谊。

本系统的不足在于三维重建方法可以更加多一些，对文中所涉及的算法也可 以进一步优化。通过比较更多的三维重建算法来完成三维重建任务，可以验证出 每个建模算法的适用情况，从而能够针对性地提升算法选择和使用效率。实验数 据集的选择可以更纯净一些，有一些数据集的噪声点过多，严重影响最终呈现结 果和准确率。降维过程对于临界值的选择可以动态改变，根据不同图像的实际情 况选取合适的阈值，从而可以提高降维的实际准确率，为扫地机器人的成功寻址 提供基础。

针对以上不足之处，我们的改进就在于应用更多三维重建方法、完善算法的   比较过程，得到更多细节上的结果，得到各种算法的适用情况，从而可以针对性   地使用对应算法。同时对于数据集的采集可以选取更优质的图像，尽可能更多地   获得有效信息， 同时减少噪声的采集。对于降维算法采取动态管制，根据每个图   像的实际情况进行选择阈值，可以考虑使用深度学习算法来找到这个合适的阈值。 同时路径规划算法我们设计了基于贪心的算法，也可以对路径规划算法做进一步优化。

真正做出扫地机器人是个非常复杂的流程，还涉及到材料的选择、机械的制 造等，但是我们的专业需要解决的核心问题是算法方面，所以我们小组完成了扫 地机器人的基础算法模型设计，从构建三维模型到降维、再到路径规划，三步走 策略基本展示了扫地机器人的工作流程。如果有机会的话，我们小组会考虑和机 械 、材料等专业的同学合作， 一起落地 、开发出一款扫地机器人。

感谢老师为我们提供这样一次机会，让我们自主选题来设计自己想研究的内 容，从而我们可以在兴趣和学习目标的双重鼓励下，圆满完成本次实验任务，虽 然本系统仍然有改进的地方，但是我们已经对这个系统有了足够的了解，这次实 验锻炼了我们的团队协作沟通的能力 、分析问题的能力 、理论联系实践的能力、信息检索能力等等 。为我们今后更复杂的工程项目打好了良好的基础。