CN109166149B - 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 - Google Patents

Abstract

本发明涉及一种融合双目相机与IMU的定位与三维线框结构重建的方法与系统。本发明在双目视觉基础上，利用分治的策略初始化并融合惯性测量信息，实施跟踪定位与建图，可以稳健地在室内室外等环境以及复杂的运动条件下运行。在精确定位的基础上，基于关键帧优化后的位姿进行三维线框的重建与迭代优化。通过局部特征与空间几何约束匹配直线段并反投影到三维空间中。通过角度与距离约束，将直线段分成不同的集合。基于分组的结果，确定拟合区域并合并直线段。最终输出三维线框结构。本发明在传统的基于视觉的定位与建图的方法上，融合多源信息提高系统的稳定性与鲁棒性。同时在关键帧处，加入线信息，稀疏表达了三维环境的结构特征，提高了计算效率。

Description

一种融合双目相机与IMU的定位与三维线框结构重建方法与 系统

技术领域

本发明属于是计算机视觉领域，特别是涉及一种融合双目相机与IMU的定位与三维线框结构重建的方法与系统。

背景技术

基于视觉的同时定位与建图是计算机视觉领域的重要研究课题。它是指运载体搭载特定的视觉传感器，在没有先验环境信息情况下，对周围环境进行感知与描述，同时估计自己的运动。完整的视觉定位建图系统可以分为五个部分：传感器数据的获取、前端视觉里程计、后端非线性优化、回环检测以及建图。与其它的测量方法相比较，视觉测量的方法具有精度高、效率高、成本低廉、系统结构简单等优势，被广泛地应用于机器人导航、无人驾驶、三维测量、增强现实以及虚拟现实等方面。根据采用的视觉传感器的类型，可以将基于视觉的定位与建图系统分为三种类型：基于单目相机的系统、基于双目相机的系统以及结合单目相机与红外传感器的系统。

基于单目的视觉测量系统无法直接得到可靠的深度信息，初始化和重建的难度大，具有尺度的不确定性。基于深度相机的视觉测量系统成本较高，体积大，有效测量范围有限，限制了应用场景。而基于双目的视觉测量系统，在一次拍摄过程中，通过视差得到环境立体信息，精度比单目高。

根据算法处理图像信息方式的不同，可以分为基于特征的方法以及直接法。直接法操作的对象是图像的像素点，直接利用图像所有的信息。但是这种方法在优化时会受到图像梯度影响，同时在外界光照条件发生变化、相机发生大尺度移动、旋转无法进行跟踪。而特征法通过提取、匹配特征点，计算相机位姿并对环境建图，在保存图像重要信息的同时，有效地减少了计算量，从而被广泛地使用。早期的单目视觉定位建图系统通过滤波的方法实现，利用概率密度函数表示不确定性，并利用观测模型和递归运算，更新存储在状态变量中的相机位姿与地图点。但是此系统的精度不够高。之后，基于关键帧的单目定位建图系统开始流行，并以此进行了一系列优化策略，进一步提高准确性与实时性。后来，为了构建稠密的三维地图，出现了基于直接法的视觉测量系统。

随着科技发展，人们对于环境感知与定位建图的性能提出了更高的要求。一方面，基于特征点的稀疏地图的构建，丢失了环境中的关键结构信息，无法形成对环境进行有效的描述。稠密的地图的构建则需要处理数量庞大的点云数据，计算代价高。另一方面，仅仅依靠单一的视觉传感器无法满足某些复杂场景下(比如，光照变化、遮挡、高光、重复纹理以及独立运动物体等)系统对于鲁棒性与稳定性的要求。目前还没有能够在复杂环境中，高效率、高精度地重建环境结构信息的系统。因此，本发明融合视觉传感器和惯性测量单元，提高定位与建图的鲁棒性，提取并处理线结构特征，对周围三维环境的结构进行感知与重建，为实际工程应用设计了一套更加稳定、精确、廉价的自动化解决方案与系统。

发明内容

本发明针对现有技术的不足，提供一种融合双目相机与IMU的定位与三维线框结构重建方法与系统，本发明对于数据采集没有特殊的要求，可以是手持、车载等方式；在数据采集的同时，即可形成线框结构的重建与优化，保证了全局结构的一致性和连续性；抗干扰能力强，可以在复杂运动条件下稳定运行；不受应用场景的限制，可以在室内与室外运行。

为了达到上述目的，本发明提供的技术方案是：一种融合双目相机与IMU的定位与三维线框结构重建系统，包括如下模块：

数据采集模块，分别对双目相机拍摄的低频的图像流以及惯性测量单元中加速计与陀螺仪采集的高频的数据进行读取与预处理；

特征提取与匹配模块，通过左右目图像特征点的提取与匹配，计算视差，恢复点在相机坐标系下的三维位置；提取图像流中关键帧以及关键帧中的直线段特征，而后基于局部特征描述子和外观、几何特征进行匹配，分别计算直线段两个端点的视差，从而恢复线在相机坐标系下的三维位置；

跟踪模块，联合视觉特征点和惯性信息估计相邻图像之间的相对运动，从而确定图像帧位姿，并将特征提取与匹配模块得到的三维点与线统一到世界坐标系下，描述局部地图；

优化模块，对跟踪模块得到的位姿与三维空间位置以及惯性测量值进行优化，计算全局一致的精确的运动轨迹与地图；同时在运载体运动的同时，对当前运动场景进行识别，判断是否与之前到访的地点形成回环，形成空间上几何强约束；

线框重建与优化模块，基于优化后的结果，将二维图像中匹配的线特征，反投影到三维空间中，并依据距离与方向阈值对空间中线段进行分组拟合，通过迭代求解，最后形成全局一致的结构描述。

本发明还提供一种融合双目相机与IMU的定位与三维线框结构重建方法，包括如下步骤：

步骤1，读取惯性测量信息流和双目视频流数据，并根据相机的参数，对图像进行几何校正，消除左右视图的畸变，使得行对准；同时在时间维度上对齐读入的惯性测量数据与双目图像数据；

步骤2，在纯双目视觉模式下运行，提取特征点和关键帧中的LSD特征线段，进行左右目图像特征匹配，并基于关键帧进行跟踪定位与建图；

步骤3，利用步骤2中估计的关键帧的位置与姿态，初始化惯性测量单元；

步骤4，完成步骤3的IMU初始化以后，重新进行特征提取与图像匹配，而后联合双目视觉与惯性测量单元进行跟踪定位与建图；

步骤5，根据步骤4中的关键帧的位姿，更新步骤2中特征线段端点的坐标，得到初始的三维线框的位置；然后，遍历三维线框中的所有线段，计算线段间距离和夹角，与角度阈值和距离阈值相比较，判断线段是否表示同一条边，将同一边的线段聚为一个组，得到不同的线段的组；

步骤6，遍历步骤5得到的线段的组的集合，分别使用各组内包含的所有线段拟合出各组所对应的边；

步骤7，所有组的线段拟合完成后，统计拟合结果数量N₂，设定拟合阈值N_delta，若|N-N₂|<N_delta(N为线段总数)，则说明所有线段拟合完毕，不存在表示同一条边的线段组合；若|N-N₂|≥N_delta，N＝N₂，返回步骤5中重新计算线段间的距离；

步骤8，输出三维线框结构。

进一步的，步骤2的具体实现方式如下，

步骤2.1，提取并匹配所有左右目图像帧的ORB特征点以及关键帧的左右目图像的LSD特征线段，通过特征点计算视差，并恢复特征点与LSD特征线段在相机坐标系下的三维空间位置；

步骤2.2，进行视觉跟踪定位，通过ORB-SLAM2跟踪线程，计算双目相机相邻帧之间的位姿变化以及特征点和特征线段在世界坐标系下的三维坐标；

步骤2.3，在跟踪线程后，将关键帧插入队列，更新关键帧之间的共视关系，剔除质量不好的地图点，对步骤2.1中反投影到空间中的地图点进行融合，构造g2o优化器，进一步优化共视关键帧的位姿，剔除冗余的关键帧并判断是否加入新的关键帧；

步骤2.4，利用DBoW2库进行回环检测、回环验证以及回环校正；

步骤2.5，固定优化的回环帧、当前关键帧及两者邻域内的帧，优化剩余帧的位姿；

步骤2.6，根据优化后的关键帧的位姿和针孔相机投影模型，计算特征点与特征线段在世界坐标系下的三维坐标。

进一步的，步骤2.1的具体实现方式如下，

步骤2.1.1，创建点特征提取匹配线程与线特征提取匹配线程；

步骤2.1.2，在点特征提取线程中，将读入的双目图像转换为灰度图像，分别提取左右目图像的ORB特征点；

步骤2.1.3，以左图为基准，在右图相对应基线上下δ_//行的宽度范围内寻找最佳匹配点，并变换到尺度对应层，根据SAD(Sum of absolute differences)算法，评估左图模板T(M×N)与右搜索图S(m×n)的相似度D(i,j)：

其中，1≤i≤m-M+1，1≤j≤n-N+1；计算搜索范围内的最高相似度D对应的像素块位置及其匹配相似度，并结合此像素块左右邻域距离为1的像素块及其匹配相似度，利用抛物线拟合得到亚像素级的最佳匹配块位置；

步骤2.1.4，计算左图像特征点和步骤2.1.3得到的对应的右图像的最佳匹配块位置的差异d＝U_L-U_R，其中U_L表示特征点在左图像的位置，U_R表示最佳匹配块在右图像的位置；根据针孔相机投影模型，先计算特征点深度

其中f为焦距，b为左右目相机的基线长度，然后计算特征点X,Y坐标，得到当前相机坐标系下位置(X,Y,Z)；

步骤2.1.5，利用开源代码框架ORB-SLAM模块，提取视频中关键帧，此时进入线特征提取与匹配线程，在关键帧处，利用LSD算法提取图像的线段特征；

步骤2.1.6，构建线段的LBD局部特征描述子，并计算线段的一元几何属性与局部外观(LBD)相似度，通过阈值筛选，得到候选匹配组；

步骤2.1.7，对于候选匹配组，首先进行线段二维投影长度值L_2d和三维空间长度值L_3d的筛选，在此基础上，基于LBD局部特征描述子进行强约束：为了减少误匹配线对，只考虑左右图像中某两条线段都互为对方最佳匹配的情况；为了进一步加强匹配的有效性，对最佳匹配的距离设置阈值，每两对最相近的匹配组合间的距离应该小于其中最佳匹配的距离的2倍；

步骤2.1.8，在对LBD描述子之间距离计算与匹配之后，通过直线段相对角度、投影比率、交点比率端点最小最大视差比率进行局部外观统计量的二次筛选；

步骤2.1.9，二次筛选后，将线段的两个端点反投影到三维空间中，并除去平行于基线的向量以及视场之外、深度超过最大深度阈值的离群向量。

进一步的，步骤2.2的具体实现方式如下，

步骤2.2.1，首先按照恒速运动假设，估计当前帧的相机位姿，搜索当前帧中观测到的前一帧的地图点，根据上一帧特征点对应的投影点的位置，缩小特征匹配的范围，得到匹配点对组，根据匹配结果，构造g2o优化器，最小化重投影误差e，优化位姿：

e＝(u,v)-Proj(T_cw×P_w)

其中，(u,v)表示特征点在待匹配二维图像上的位置，T_cw表示当前帧的位姿，P_w表示特征点的三维空间中的世界坐标；

步骤2.2.2，如果恒速运动模型失败，则使用跟踪参考帧的模型，将上一帧的位姿作为当前帧的初始位姿，通过词袋BoW在参考帧中搜索当前帧特征点的匹配点，构造g2o优化器，优化位姿；

步骤2.2.3，如果跟踪参考帧的模型失败了，则使用重定位模型，计算BoW向量，找到与当前帧相似的候选关键帧，根据ORB匹配点，计算关键帧所对应的地图点，然后通过EPnP算法估计姿态，构造g2o优化器，优化位姿。

进一步的，步骤3中初始化惯性测量单元的具体实现方式如下，

首先利用预积分模型表示惯性测量单元Δt_i,i+1时间内两连续关键帧i,i+1之间运动：

其中，W为世界坐标系，B为惯性测量坐标系，a_b为加速度，ω_B为角速度，g_w为重力加速度，b_a为加速度偏差，b_g为陀螺仪偏差，R_WB为B位置对应于W的姿态矩阵，v_WB为B对应于W中的速度，p_WB为B对应于W中的位置，ΔR_i,i+1表示陀螺仪测量的i到i+1关键帧时间内的姿态变化量，

雅克比矩阵表示(·)关于b_g、b_a一阶变化估计；然后利用分治的策略，分别对陀螺仪偏差、重力加速度、加速度偏差进行估计与优化，最后计算帧间的速度；具体包括以下子步骤：

步骤3.1，陀螺仪偏差可以从两连续的相邻的关键帧i,i+1之间的相对方向估计，系统优化一个陀螺仪偏差b_g使得双目视觉估计N个连续关键帧之间的相对方向与陀螺仪积分所得到的相对方向的差值最小化，

步骤3.2，重力估计，忽略此时刻的加速度的偏移，得到Δt时间内连续两关键帧相对关系：

其中，C表示相机坐标系，

R_CB可以通过校正文件获得，而后解得g_w值记为

步骤3.3，加速度偏差估计，惯性系统参考系I中重力方向向量为

和已经计算得到的重力方向

可以计算旋转矩阵R_WI：

重力加速度

其中G表示的是当地重力大小，得到：

其中，绕Z轴旋转的扰动项δθ＝[δθ_xy 0]^T，

从而解得重力方向修正值δθ_xy,加速度偏差b_a；

步骤3.4，根据连续三个关键帧i,i+1,i+2的关系式以及计算得到的b_g,b_a,δθ_xy，代入预积分模型，估计速度v_WB。

进一步的，步骤4的具体实现方式如下，

步骤4.1，将步骤2的视觉跟踪定位中子步骤2.2.1的恒速模型替换为惯性测量单元预积分得到的速度值v_WB，其余跟踪模型保持不变；

步骤4.2，跟踪得到相机位姿后，局部地图中的三维点被投影到像面，与帧中提取的关键点进行匹配，通过最小化匹配点的重投影误差与惯性测量误差来优化当前帧；

步骤4.2.1，地图尚未更新阶段，系统通过上一关键帧i的参数以及当前地图点的重投影参数对当前帧j进行估计：

其中，k为给定的匹配结果，E_proj(k,j)为当前帧左图像特征点的重投影误差，E_IMU(i,j)为IMU在帧i,j之间的测量误差；通过将g2o牛顿法得到优化的结果作为下次优化的先验值E_prior(j)；

步骤4.2.2，在步骤4.2.1基础上，加入E_proj(j)，θ^*＝argmin_θ(∑_kE_proj(k,j+1)+E_IMU(j,j+1)+E_prior(j))，将帧j和帧j+1进行联合优化，优化以后，帧j被边缘化处理，不再参与后续的跟踪优化中，而帧j+1的值将作为下次优化的初始值，如此循环往复直到地图被更新为止(检测到闭环或者开始局部建图)；

步骤4.3，在视觉局部建图的基础上，控制关键帧之间的时间间隔，固定当前共视区域外的关键帧，计算当前N个关键帧所观测的局部地图中的地图点的重投影误差；N+1关键帧将约束之前N个关键帧的惯性测量值，最后通过光束法平差优化惯性测量误差与重投影误差的和，估计N个关键帧的位姿以及惯性测量值；

步骤4.4，利用DBoW2库进行回环检测、回环验证以及根据重投影误差值进行回环校正；

步骤4.5，计算全局(所有帧)的重投影误差与惯性测量误差，并构造g2o，进行全局的位姿优化以及三维位置的优化。

进一步的，步骤5中得到不同的线段的组的具体实现方式如下，

步骤5.1，遍历所有线段(i＝1,j＝1)，计算线段间距离，通过距离阈值T_d约束，分为以下子步骤：

步骤5.1.1，若i≤N(线段总数)时，计算第i条线段所在直线与第j条线段的两个端点的距离之和D：

D＝d1+d2

其中P_isx、P_isy、P_isz和P_iex、P_iey、P_iez代表第i条线段的起点和终点的坐标，P_jsx、P_jsy、P_jsz和P_jex、P_jey、P_jez代表第j条线段的起点和终点的坐标，

代表第i条线段的向量，d1、d2分别为第j条线段的起点和终点到第i条线段所在直线距离；若i>N，则进入步骤6；

步骤5.1.2，比较D和距离阈值T_d，小于T_d则进行下一步骤；大于T_d则j＝j+1，若j≤N返回步骤5.1.1，若j>N则j＝1，i＝i+1，返回步骤5.1.1；

步骤5.2，计算线段间夹角，通过夹角阈值T_α约束，分为以下子步骤：

步骤5.2.1，计算第i条线段所在直线与第j条线段的夹角：

其中

代表第j条线段的向量，cos^-1代表反余弦函数；

步骤5.2.2，比较α和夹角阈值T_α，小于T_α则进行下一步骤；大于T_α则j＝j+1，若j≤N返回步骤5.1.1，若j>N则j＝1，i＝i+1，返回步骤5.1.1；

步骤5.3，线段i,j通过距离阈值和夹角阈值约束，将第j条线段与第i条线段归为一组，j＝j+1，若j≤N返回步骤5.1.1，若j>N则j＝1，i＝i+1，返回步骤5.1.1。

进一步的，步骤6的具体实现包括如下步骤，

步骤6.1，获取每个组内所有线段的端点，基于M估计(M-estimators)的方法进行直线迭代加权拟合，分为以下几个子步骤：

步骤6.1.1，选取Huber’s M-estimators，对应的ρ函数为：

其中，r表示自变量；

步骤6.1.2，使用选取的ρ函数，构造参数求解模型F：

其中n为每组线段的总数，ρ为选择的Distance function，a_i、L为观测值，X为直线方程中的未知量，空间直线可以表示为：

待求参数为X₀、Y₀、Z₀、m、n，观测值(每条线段的端点坐标)为X、Y、Z

将空间直线转化为矩阵形式可得：

带入上面的求解模型：

n条线段得到2n个M-估计方程，通过模型残差倒数加权最小二乘迭代拟合求解(X₀,Y₀,Z₀),(m,n,1)；

步骤6.1.3，解得直线通过的三维点坐标P:(X₀,Y₀,Z₀)以及直线的方向向量l_direction:(m,n,1)；

步骤6.2，在步骤6.1将同组线段进行直线拟合，确定直线的范围，得到表示边的线段，分为以下子步骤：

步骤6.2.1，遍历同组内所有线段的端点，找到距离最远的两个点P1、P2；

步骤6.2.2，根据拟合直线的方向向量l_direction，提取P1、P2对应方向向量主方向(绝对值最大的)的值，带入直线方程中，解得表示边框线段的两个端点，从而确定线段的空间位置。

本发明在双目视觉基础上，利用分治的策略初始化并融合惯性测量信息，实施跟踪定位与建图，可以稳健地在室内室外等环境以及复杂的运动条件下运行。在精确定位的基础上，本发明基于关键帧优化后的位姿进行三维线框的重建与迭代优化。通过局部特征与空间几何约束匹配直线段并反投影到三维空间中。通过角度与距离约束，将直线段分成不同的集合。基于分组的结果，确定拟合区域并合并直线段。最终输出三维线框结构。本发明在传统的基于视觉的定位与建图的方法上，融合多源信息提高系统的稳定性与鲁棒性。同时在关键帧处，加入线信息，稀疏表达了三维环境的结构特征，同时提高了计算效率。

附图说明

图1为本发明实施例的具体流程框图。

图2为本发明实施例中实验数据集部分场景(图2(a))，以及视觉跟踪定位结果图(图2(b)和(c))。

图3为本发明实施例最终得到的优化的轨迹位置数据平面投影结果(图3(a)和(b))、三维结构的平面投影图(图3(c)和(d))以及重建的三维线框的三维视图(图3(e)—(g))。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步说明。

本发明实施例提供的一种融合双目相机与IMU的定位与三维线框结构重建系统，包括如下模块：

数据采集模块，分别对双目相机拍摄的低频的图像流以及惯性测量单元中加速计与陀螺仪采集的高频的数据进行读取与预处理；

特征提取与匹配模块，通过左右目图像特征点的提取与匹配，计算视差，恢复点在相机坐标系下的三维位置；提取图像流中关键帧以及关键帧中的直线段特征，而后基于局部特征描述子和外观、几何特征进行匹配，分别计算直线段两个端点的视差，从而恢复线在相机坐标系下的三维位置。

跟踪模块，联合视觉特征点和惯性信息估计相邻图像之间的相对运动，从而确定图像帧位姿，并将特征提取与匹配模块得到的三维点与线统一到世界坐标系下，描述局部地图；

优化模块，对跟踪模块得到的位姿与三维空间位置以及惯性测量值进行优化，计算全局一致的精确的运动轨迹与地图；同时在运载体运动的同时，对当前运动场景进行识别，判断是否与之前到访的地点形成回环，形成空间上几何强约束；

线框重建与优化模块，基于优化后的结果，将二维图像中匹配的线特征，反投影到三维空间中，并依据距离与方向阈值对空间中线段进行分组拟合，通过迭代求解，最后形成全局一致的结构描述。

如图1所示，本发明实施例提供的一种融合双目相机与IMU的定位与三维线框结构重建方法，具体包括如下步骤：

步骤1，读取惯性测量信息流和双目视频流数据(实验数据集部分场景见图2-(a))，并根据相机的参数，对图像进行几何校正，消除左右视图的畸变，使得行对准；同时在时间维度上对齐读入的惯性测量数据与双目图像数据；

步骤2，在纯双目视觉模式下运行，提取特征点和关键帧中的LSD特征线段，进行左右目图像特征匹配，并基于关键帧进行跟踪定位与建图；

步骤2.1，提取并匹配所有左右目图像帧的ORB特征点以及关键帧的左右目图像的LSD特征线段，通过特征点计算视差，并恢复特征点与LSD特征线段在相机坐标系下的三维空间位置；

步骤2.1.1，创建点特征提取匹配线程与线特征提取匹配线程；

步骤2.1.2，在点特征提取线程中，将读入的双目图像转换为灰度图像，分别提取左右目图像的ORB特征点；

步骤2.1.3，以左图为基准，在右图相对应基线上下δ_//行的宽度范围内寻找最佳匹配点，并变换到尺度对应层，根据SAD(Sum of absolute differences)算法，评估左图模板T(M×N)与右搜索图S(m×n)的相似度D(i,j)：

其中，1≤i≤m-M+1，1≤j≤n-N+1；计算搜索范围内的最高相似度D对应的像素块位置及其匹配相似度，并结合此像素块左右邻域距离为1的像素块及其匹配相似度，利用抛物线拟合得到亚像素级的最佳匹配块位置；

步骤2.1.4，计算左图像特征点和步骤2.1.3得到的对应的右图像的最佳匹配块位置的差异d＝U_L-U_R，其中U_L表示特征点在左图像的位置，U_R表示最佳匹配块在右图像的位置；根据针孔相机投影模型，先计算特征点深度

其中f为焦距，b为左右目相机的基线长度，然后计算特征点X,Y坐标，得到当前相机坐标系下位置(X,Y,Z)；

步骤2.1.5，利用开源代码框架ORB-SLAM模块，提取视频中关键帧，此时进入线特征提取与匹配线程，在关键帧处，利用LSD算法提取图像的线段特征；

步骤2.1.6，构建线段的LBD局部特征描述子，并计算线段的一元几何属性与局部外观(LBD)相似度，通过阈值筛选，得到候选匹配组[1]。

[1]Zhang L,Koch R.An efficient and robust line segment matchingapproach based on LBD descriptor and pairwise geometric consistency[J].Journal of Visual Communication&Image Representation,2013,24(7):794-805.

步骤2.1.7，对于候选匹配组，首先进行线段二维投影长度值L_2d和三维空间长度值L_3d的筛选(L_2d>L₁ pixels，L_3d>L₂m，这里取L₁＝0.05*min(I_w，I_h)，其中I_w表示图像的宽度，I_h表示图像的高度；L₂＝0.5m)，在此基础上，基于LBD局部特征描述子进行强约束：为了减少误匹配线对，只考虑左右图像中某两条线段都互为对方最佳匹配的情况；为了进一步加强匹配的有效性，对最佳匹配的距离设置阈值，每两对最相近的匹配组合间的距离应该小于其中最佳匹配的距离的2倍；

步骤2.1.8，在对LBD描述子之间距离计算与匹配之后，通过直线段相对角度θ₁₂(cos(θ₁₂)>0.75)、投影比率(θ_proj>0.85)、交点比率端点最小最大视差比率(R_disp>0.6)进行局部外观统计量的二次筛选；

步骤2.1.9，二次筛选后，将线段的两个端点反投影到三维空间中，除去平行于基线的向量以及视场之外、深度超过最大深度阈值d>Max_Depth，(这里Max_Depth取5m)的离群向量；

步骤2.2，在分别进行特征点和特征线段的立体匹配后，开始视觉跟踪定位。通过ORB-SLAM2跟踪线程，计算双目相机相邻帧之间的位姿变化以及特征点、特征线段在世界坐标系下的三维坐标(运行结果见图2-(b)与(c)，其中图2-(b)表示当前系统输入的左图像，方框表示匹配成功的左图像特征点，图2-(c)表示系统根据关键帧位姿恢复的线的三维坐标)；

步骤2.2.1，首先按照恒速运动假设，估计当前帧的相机位姿，搜索当前帧中观测到的前一帧的地图点，根据上一帧特征点对应的投影点的位置，缩小特征匹配的范围，得到匹配点对组，根据匹配结果，构造g2o优化器，最小化重投影误差e，优化位姿：

e＝(u,v)-Proj(T_cw×P_w)

其中，(u,v)表示特征点在待匹配二维图像上的位置，T_cw表示当前帧的位姿，P_w表示特征点的三维空间中的世界坐标；

步骤2.2.2，如果恒速运动模型失败，则使用跟踪参考帧的模型，将上一帧的位姿作为当前帧的初始位姿，通过词袋BoW在参考帧中搜索当前帧特征点的匹配点，构造g2o优化器，优化位姿；

步骤2.2.3，如果跟踪参考帧的模型失败了，则使用重定位模型，计算BoW向量，找到与当前帧相似的候选关键帧，根据ORB匹配点，计算关键帧所对应的地图点，然后通过EPnP算法估计姿态，构造g2o优化器，优化位姿；

步骤2.3，在跟踪线程后，将关键帧插入队列，更新关键帧之间的共视关系，剔除质量不好的地图点(这里的质量不好的地图点是指：可跟踪该点的图像帧占可观测图像帧比例小于

可观测到该点的关键帧数目小于N_keyframe，这里

N_keyframe＝3)，对步骤2.1中反投影到空间中的地图点进行融合，构造g2o优化器，进一步优化共视关键帧的位姿，剔除冗余的关键帧并判断是否加入新的关键帧；

步骤2.4，利用DBoW2库进行回环检测、回环验证以及回环校正；

步骤2.5，固定优化的回环帧、当前关键帧及两者邻域内的帧，优化剩余帧的位姿；

步骤2.6，根据优化后的关键帧的位姿和针孔相机投影模型，计算特征点与特征线段在世界坐标系下的三维坐标。

步骤3，利用步骤2中估计的关键帧的位置与姿态，初始化惯性测量单元；

惯性测量单元Δt_i,i+1时间内两连续关键帧i,i+1之间运动，可以利用预积分模型[2]表示为：

其中，W为世界坐标系，B为惯性测量坐标系，a_b为加速度，ω_B为角速度，g_w为重力加速度，b_a为加速度偏差，b_g为陀螺仪偏差，R_WB为B位置对应于W的姿态矩阵，v_WB为B对应于W中的速度，p_WB为B对应于W中的位置，ΔR_i,i+1表示陀螺仪测量的i到i+1关键帧时间内的姿态变化量，

雅克比矩阵表示(·)关于b_g、b_a一阶变化估计；利用分治的策略，分别对陀螺仪偏差、重力加速度、加速度偏差进行估计与优化，最后计算帧间的速度。

[2]Forster C,Carlone L,Dellaert F,et al.On-Manifold Preintegrationfor Real-Time Visual--Inertial Odometry[J].IEEE Transactions on Robotics,2017,33(1):1-21.

具体包括以下子步骤：

步骤3.1，陀螺仪偏差可以从两连续的相邻的关键帧i,i+1之间的相对方向估计，系统优化一个陀螺仪偏差b_g使得双目视觉估计N个连续关键帧之间的相对方向与陀螺仪积分所得到的相对方向的差值最小化，

步骤3.2，重力估计，忽略此时刻的加速度的偏移，得到Δt时间内连续两关键帧相对关系：

其中，C表示相机坐标系，

R_CB可以通过校正文件获得，而后解得g_w值记为

步骤3.3，加速度偏差估计，惯性系统参考系I中重力方向向量为

和已经计算得到的重力方向

可以计算旋转矩阵R_WI：

重力加速度

其中G表示的是当地重力大小，得到：

其中，绕Z轴旋转的扰动项δθ＝[δθ_xy 0]^T，v_WB可以用连续三个关键帧之间关系消除，得到：

从而解得重力方向修正值δθ_xy,加速度偏差b_a；

步骤3.4，根据连续三个关键帧i,i+1,i+2的关系式以及计算得到的b_g,b_a,δθ_xy，代入预积分模型，估计速度v_WB；

步骤4，在系统完成步骤3的IMU初始化以后，仿照步骤2.1，提取与匹配图像特征，而后联合双目视觉与惯性测量单元进行跟踪定位与建图。

步骤4.1，将步骤2的视觉跟踪定位中子步骤2.2.1的恒速模型替换为惯性测量单元预积分得到的速度值v_WB，其余跟踪模型保持不变。

步骤4.2，跟踪得到相机位姿后，局部地图中的三维点被投影到像面，与帧中提取的关键点进行匹配，通过最小化匹配点的重投影误差与惯性测量误差来优化当前帧；

步骤4.2.1，地图尚未更新阶段，系统通过上一关键帧i的参数以及当前地图点的重投影参数对当前帧j进行估计：

其中，k为给定的匹配结果，E_proj(k,j)为当前帧左图像特征点的重投影误差，E_IMU(i,j)为IMU在帧i,j之间的测量误差；通过将g2o牛顿法得到优化的结果(位姿估计和H矩阵)作为下次优化的先验值E_prior(j)[3]；

[3]Mur-Artal R,Tardós J D.Visual-Inertial Monocular SLAM With MapReuse[J].IEEE Robotics&Automation Letters,2017,2(2):796-803.

步骤4.2.2，在步骤4.2.1基础上，加入E_proj(j)，θ^*＝argmin_θ(∑_kE_proj(k,j+1)+E_IMU(j,j+1)+E_prior(j))，将帧j和帧j+1进行联合优化，优化以后，帧j被边缘化处理，不再参与后续的跟踪优化中，而帧j+1的值将作为下次优化的初始值，如此循环往复直到地图被更新为止(检测到闭环或者开始局部建图)；此时，当前帧关联的上一关键帧将代替上一帧重新开始新一轮循环优化过程中。

步骤4.3，在视觉局部建图的基础上，控制关键帧之间的时间间隔T<0.5s，固定当前共视区域外的关键帧，计算当前N个关键帧所观测的局部地图中的地图点的重投影误差；N+1关键帧将约束之前N个关键帧的惯性测量值，最后通过光束法平差优化惯性测量误差与重投影误差的和，估计N个关键帧的位姿以及惯性测量值；

步骤4.4，利用DBoW2库进行回环检测、回环验证以及根据重投影误差值进行回环校正。

步骤4.5，计算全局(所有帧)的重投影误差与惯性测量误差，并构造g2o，进行全局的位姿优化以及三维位置的优化；最终得到的优化的轨迹位置数据平面投影结果见图3-(a)(b)。

步骤5，根据步骤4中的关键帧的位姿，更新步骤2中特征线段端点的坐标，得到初始的三维线框的位置；然后，遍历所有线段，计算线段间距离和夹角，与角度阈值和距离阈值相比较，判断线段是否表示同一条边，将同一边的线段聚为一个组，得到不同的线段的组；

步骤5.1，遍历所有线段(i＝1,j＝1)，计算线段间距离，通过距离阈值T_d约束，分为以下子步骤：

步骤5.1.1，若i≤N(线段总数)时，计算第i条线段所在直线与第j条线段的两个端点的距离之和D：

D＝d1+d2

其中P_isx、P_isy、P_isz和P_iex、P_iey、P_iez代表第i条线段的起点和终点的坐标，P_jsx、P_jsy、P_jsz和P_jex、P_jey、P_jez代表第j条线段的起点和终点的坐标，

代表第i条线段的向量，d1、d2分别为第j条线段的起点和终点到第i条线段所在直线距离；若i>N，则进入步骤6；

步骤5.1.2，比较D和距离阈值T_d，小于T_d则进行下一步骤；大于T_d则j＝j+1，若j≤N返回步骤5.1.1，若j>N则j＝1，i＝i+1，返回步骤5.1.1；

步骤5.2，计算线段间夹角，通过夹角阈值T_α约束，分为以下子步骤：

步骤5.2.1，计算第i条线段所在直线与第j条线段的夹角：

其中

代表第j条线段的向量，cos^-1代表反余弦函数；

步骤5.2.2，比较α和夹角阈值T_α，小于T_α则进行下一步骤；大于T_α则j＝j+1，若j≤N返回步骤5.1.1，若j>N则j＝1，i＝i+1，返回步骤5.1.1；

步骤5.3，线段i,j通过距离阈值和夹角阈值约束，将第j条线段与第i条线段归为一组，j＝j+1，若j≤N返回步骤5.1.1，若j>N则j＝1，i＝i+1，返回步骤5.1.1；

步骤6，遍历步骤5得到的线段的组的集合，分别使用各组内包含的所有线段拟合出各组所对应的边；

步骤6.1，获取每个组内所有线段的端点，基于M估计(M-estimators)的方法进行直线迭代加权拟合，分为以下几个子步骤：

步骤6.1.1，选取M-估计的ρ函数，在这里选取Huber’s M-estimators，对应的ρ函数为：

其中，r表示自变量。

步骤6.1.2，使用选取的ρ函数，构造参数求解模型F：

其中n为每组线段的总数，ρ为选择的Distance function，a_i、L为观测值，X为直线方程中的未知量，空间直线可以表示为：

待求参数为X₀、Y₀、Z₀、m、n，观测值(每条线段的端点坐标)为X、Y、Z

将空间直线转化为矩阵形式可得：

带入上面模型：

n条线段得到2n个M-估计方程，通过模型残差倒数加权最小二乘迭代拟合求解(X₀,Y₀,Z₀),(m,n,1)。

步骤6.1.3，解得直线通过的三维点坐标P:(X₀,Y₀,Z₀)以及直线的方向向量l_direction:(m,n,1)；

步骤6.2，在步骤6.1将同组线段进行直线拟合后，确定直线的范围，得到表示边的线段，分为以下子步骤：

步骤6.2.1，遍历同组内所有线段的端点，找到距离最远的两个点P1、P2；

步骤6.2.2，根据拟合直线的方向向量l_direction，提取P1、P2对应方向向量主方向(绝对值最大的)的值(如P1_y、P2_y)，带入直线方程中，解得表示边框线段的两个端点，从而确定线段的空间位置；

步骤7，所有组的线段拟合完成后，统计拟合结果数量N₂，设定拟合阈值N_delta，若|N-N₂|<N_delta(N为线段总数)，则说明所有线段拟合完毕，不存在表示同一条边的线段组合；若|N-N₂|≥N_delta，N＝N₂，返回步骤5.1中重新计算线段间的距离；

步骤8，输出三维线框结构。得到的三维结构的平面投影图见图3-(c)(d)，线框重建三维视图见图3-(e)(f)(g)。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (7)

1.一种融合双目相机与IMU的定位与三维线框结构重建方法，其特征在于，包括如下步骤：

步骤1，读取惯性测量信息流和双目视频流数据，并根据相机的参数，对图像进行几何校正，消除左右视图的畸变，使得行对准；同时在时间维度上对齐读入的惯性测量数据与双目图像数据；

步骤2，在纯双目视觉模式下运行，提取特征点和关键帧中的LSD特征线段，进行左右目图像特征匹配，并基于关键帧进行跟踪定位与建图；

步骤3，利用步骤2中估计的关键帧的位置与姿态，初始化惯性测量单元；

步骤4，完成步骤3的IMU初始化以后，重新进行特征提取与图像匹配，而后联合双目视觉与惯性测量单元进行跟踪定位与建图；

步骤5，根据步骤4中的关键帧的位姿，更新步骤2中特征线段端点的坐标，得到初始的三维线框的位置；然后，遍历三维线框中的所有线段，计算线段间距离和夹角，与角度阈值和距离阈值相比较，判断线段是否表示同一条边，将同一边的线段聚为一个组，得到不同的线段的组；

步骤6，遍历步骤5得到的线段的组的集合，分别使用各组内包含的所有线段拟合出各组所对应的边；具体实现包括如下步骤，

步骤6.1，获取每个组内所有线段的端点，基于M估计的方法进行直线迭代加权拟合，分为以下几个子步骤：

步骤6.1.1，选取Huber’s M-estimators，对应的ρ函数为：

其中，r表示自变量；

步骤6.1.2，使用选取的ρ函数，构造参数求解模型F：

其中n为每组线段的总数，ρ为选择的Distance function，a_i、L为观测值，X为直线方程中的未知量，空间直线可以表示为：

待求参数为X₀、Y₀、Z₀、m、n，观测值，即每条线段的端点坐标为X、Y、Z；

将空间直线转化为矩阵形式可得：

带入上面的求解模型：

n条线段得到2n个M-估计方程，通过模型残差倒数加权最小二乘迭代拟合求解(X₀,Y₀,Z₀),(m,n,1)；

步骤6.1.3，解得直线通过的三维点坐标P:(X₀,Y₀,Z₀)以及直线的方向向量l_direction:(m,n,1)；

步骤6.2，在步骤6.1将同组线段进行直线拟合，确定直线的范围，得到表示边的线段，分为以下子步骤：

步骤6.2.1，遍历同组内所有线段的端点，找到距离最远的两个点P1、P2；

步骤6.2.2，根据拟合直线的方向向量l_direction，提取P1、P2对应方向向量主方向的值，带入直线方程中，解得表示边框线段的两个端点，从而确定线段的空间位置；

步骤7，所有组的线段拟合完成后，统计拟合结果数量N₂，设定拟合阈值N_delta，若|N-N₂|<N_delta，N为线段总数，则说明所有线段拟合完毕，不存在表示同一条边的线段组合；若|N-N₂|≥N_delta，N＝N₂，返回步骤5中重新计算线段间的距离；

步骤8，输出三维线框结构。

2.如权利要求1所述的一种融合双目相机与IMU的定位与三维线框结构重建方法，其特征在于：步骤2的具体实现方式如下，

步骤2.1，提取并匹配所有左右目图像帧的ORB特征点以及关键帧的左右目图像的LSD特征线段，通过特征点计算视差，并恢复特征点与LSD特征线段在相机坐标系下的三维空间位置；

步骤2.2，进行视觉跟踪定位，通过ORB-SLAM2跟踪线程，计算双目相机相邻帧之间的位姿变化以及特征点和特征线段在世界坐标系下的三维坐标；

步骤2.3，在跟踪线程后，将关键帧插入队列，更新关键帧之间的共视关系，剔除质量不好的地图点，对步骤2.1中反投影到空间中的地图点进行融合，构造g2o优化器，进一步优化共视关键帧的位姿，剔除冗余的关键帧并判断是否加入新的关键帧；

步骤2.4，利用DBoW2库进行回环检测、回环验证以及回环校正；

步骤2.5，固定优化的回环帧、当前关键帧及两者邻域内的帧，优化剩余帧的位姿；

步骤2.6，根据优化后的关键帧的位姿和针孔相机投影模型，计算特征点与特征线段在世界坐标系下的三维坐标。

3.如权利要求2所述的一种融合双目相机与IMU的定位与三维线框结构重建方法，其特征在于：步骤2.1的具体实现方式如下，

步骤2.1.1，创建点特征提取匹配线程与线特征提取匹配线程；

步骤2.1.2，在点特征提取线程中，将读入的双目图像转换为灰度图像，分别提取左右目图像的ORB特征点；

步骤2.1.3，以左图为基准，在右图相对应基线上下δ_//行的宽度范围内寻找最佳匹配点，并变换到尺度对应层，根据Sum of absolute differences算法，评估左图模板T与右搜索图S的相似度D(i,j)，其中左图模板T的大小为M×N，右搜索图S的大小为m×n：

其中，1≤i≤m-M+1，1≤j≤n-N+1；计算搜索范围内的最高相似度D对应的像素块位置及其匹配相似度，并结合此像素块左右邻域距离为1的像素块及其匹配相似度，利用抛物线拟合得到亚像素级的最佳匹配块位置；

步骤2.1.4，计算左图像特征点和步骤2.1.3得到的对应的右图像的最佳匹配块位置的差异d＝U_L-U_R，其中U_L表示特征点在左图像的位置，U_R表示最佳匹配块在右图像的位置；根据针孔相机投影模型，先计算特征点深度

其中f为焦距，b为左右目相机的基线长度，然后计算特征点X,Y坐标，得到当前相机坐标系下位置(X,Y,Z)；

步骤2.1.5，利用开源代码框架ORB-SLAM模块，提取视频中关键帧，此时进入线特征提取与匹配线程，在关键帧处，利用LSD算法提取图像的线段特征；

步骤2.1.6，构建线段的LBD局部特征描述子，并计算线段的一元几何属性与局部外观相似度，通过阈值筛选，得到候选匹配组；

步骤2.1.7，对于候选匹配组，首先进行线段二维投影长度值L_2d和三维空间长度值L_3d的筛选，在此基础上，基于LBD局部特征描述子进行强约束：为了减少误匹配线对，只考虑左右图像中某两条线段都互为对方最佳匹配的情况；为了进一步加强匹配的有效性，对最佳匹配的距离设置阈值，每两对最相近的匹配组合间的距离应该小于其中最佳匹配的距离的2倍；

步骤2.1.8，在对LBD描述子之间距离计算与匹配之后，通过直线段相对角度、投影比率、交点比率端点最小最大视差比率进行局部外观统计量的二次筛选；

步骤2.1.9，二次筛选后，将线段的两个端点反投影到三维空间中，并除去平行于基线的向量以及视场之外、深度超过最大深度阈值的离群向量。

4.如权利要求3所述的一种融合双目相机与IMU的定位与三维线框结构重建方法，其特征在于：步骤2.2的具体实现方式如下，

步骤2.2.1，首先按照恒速运动假设，估计当前帧的相机位姿，搜索当前帧中观测到的前一帧的地图点，根据上一帧特征点对应的投影点的位置，缩小特征匹配的范围，得到匹配点对组，根据匹配结果，构造g2o优化器，最小化重投影误差e，优化位姿：

e＝(u,v)-Proj(T_cw×P_w)

其中，(u,v)表示特征点在待匹配二维图像上的位置，T_cw表示当前帧的位姿，P_w表示特征点的三维空间中的世界坐标；

步骤2.2.2，如果恒速运动模型失败，则使用跟踪参考帧的模型，将上一帧的位姿作为当前帧的初始位姿，通过词袋BoW在参考帧中搜索当前帧特征点的匹配点，构造g2o优化器，优化位姿；

步骤2.2.3，如果跟踪参考帧的模型失败了，则使用重定位模型，计算BoW向量，找到与当前帧相似的候选关键帧，根据ORB匹配点，计算关键帧所对应的地图点，然后通过EPnP算法估计姿态，构造g2o优化器，优化位姿。

5.如权利要求4所述的一种融合双目相机与IMU的定位与三维线框结构重建方法，其特征在于：步骤3中初始化惯性测量单元的具体实现方式如下，

首先利用预积分模型表示惯性测量单元Δt_i,i+1时间内两连续关键帧i,i+1之间运动：

其中，W为世界坐标系，B为惯性测量坐标系，a_b为加速度，ω_B为角速度，g_w为重力加速度，b_a为加速度偏差，b_g为陀螺仪偏差，R_WB为B位置对应于W的姿态矩阵，v_WB为B对应于W中的速度，p_WB为B对应于W中的位置，ΔR_i,i+1表示陀螺仪测量的i到i+1关键帧时间内的姿态变化量，

雅克比矩阵表示(·)关于b_g、b_a一阶变化估计；然后利用分治的策略，分别对陀螺仪偏差、重力加速度、加速度偏差进行估计与优化，最后计算帧间的速度；具体包括以下子步骤：

步骤3.1，陀螺仪偏差可以从两连续的相邻的关键帧i,i+1之间的相对方向估计，系统优化一个陀螺仪偏差b_g使得双目视觉估计N个连续关键帧之间的相对方向与陀螺仪积分所得到的相对方向的差值最小化，

步骤3.2，重力估计，忽略此时刻的加速度的偏移，得到Δt时间内连续两关键帧相对关系：

其中，C表示相机坐标系，

R_CB可以通过校正文件获得，而后解得g_w值记为

步骤3.3，加速度偏差估计，惯性系统参考系I中重力方向向量为

和已经计算得到的重力方向

可以计算旋转矩阵R_WI：

重力加速度

其中G表示的是当地重力大小，得到：

其中，绕Z轴旋转的扰动项δθ＝[δθ_xy 0]^T，

从而解得重力方向修正值δθ_xy,加速度偏差b_a；

步骤3.4，根据连续三个关键帧i,i+1,i+2的关系式以及计算得到的b_g,b_a,δθ_xy，代入预积分模型，估计速度v_WB。

6.如权利要求5所述的一种融合双目相机与IMU的定位与三维线框结构重建方法，其特征在于：步骤4的具体实现方式如下，

步骤4.1，将步骤2的视觉跟踪定位中子步骤2.2.1的恒速模型替换为惯性测量单元预积分得到的速度值v_WB，其余跟踪模型保持不变；

步骤4.2，跟踪得到相机位姿后，局部地图中的三维点被投影到像面，与帧中提取的关键点进行匹配，通过最小化匹配点的重投影误差与惯性测量误差来优化当前帧；

步骤4.2.1，地图尚未更新阶段，系统通过上一关键帧i的参数以及当前地图点的重投影参数对当前帧j进行估计：

其中，k为给定的匹配结果，E_proj(k,j)为当前帧左图像特征点的重投影误差，E_IMU(i,j)为IMU在帧i,j之间的测量误差；通过将g2o牛顿法得到优化的结果作为下次优化的先验值E_prior(j)；

步骤4.2.2，在步骤4.2.1基础上，加入E_proj(j)，θ^*＝argmin_θ(∑_kE_proj(k,j+1)+E_IMU(j,j+1)+E_prior(j))，将帧j和帧j+1进行联合优化，优化以后，帧j被边缘化处理，不再参与后续的跟踪优化中，而帧j+1的值将作为下次优化的初始值，如此循环往复直到地图被更新为止；

步骤4.3，在视觉局部建图的基础上，控制关键帧之间的时间间隔，固定当前共视区域外的关键帧，计算当前N个关键帧所观测的局部地图中的地图点的重投影误差；N+1关键帧将约束之前N个关键帧的惯性测量值，最后通过光束法平差优化惯性测量误差与重投影误差的和，估计N个关键帧的位姿以及惯性测量值；

步骤4.4，利用DBoW2库进行回环检测、回环验证以及根据重投影误差值进行回环校正；

步骤4.5，计算全局所有帧的重投影误差与惯性测量误差，并构造g2o，进行全局的位姿优化以及三维位置的优化。

7.如权利要求1所述的一种融合双目相机与IMU的定位与三维线框结构重建方法，其特征在于：步骤5中得到不同的线段的组的具体实现方式如下，

步骤5.1，遍历所有线段，计算线段间距离，通过距离阈值T_d约束，分为以下子步骤：

步骤5.1.1，若i≤N时，计算第i条线段所在直线与第j条线段的两个端点的距离之和D：

D＝d1+d2

其中P_isx、P_isy、P_isz和P_iex、P_iey、P_iez代表第i条线段的起点和终点的坐标，P_jsx、P_jsy、P_jsz和P_jex、P_jey、P_jez代表第j条线段的起点和终点的坐标，

代表第i条线段的向量，d1、d2分别为第j条线段的起点和终点到第i条线段所在直线距离；若i>N，则进入步骤6；

步骤5.1.2，比较D和距离阈值T_d，小于T_d则进行下一步骤；大于T_d则j＝j+1，若j≤N返回步骤5.1.1，若j>N则j＝1，i＝i+1，返回步骤5.1.1；

步骤5.2，计算线段间夹角，通过夹角阈值T_α约束，分为以下子步骤：

步骤5.2.1，计算第i条线段所在直线与第j条线段的夹角：

其中

代表第j条线段的向量，cos^-1代表反余弦函数；

步骤5.2.2，比较α和夹角阈值T_α，小于T_α则进行下一步骤；大于T_α则j＝j+1，若j≤N返回步骤5.1.1，若j>N则j＝1，i＝i+1，返回步骤5.1.1；

步骤5.3，线段i,j通过距离阈值和夹角阈值约束，将第j条线段与第i条线段归为一组，j＝j+1，若j≤N返回步骤5.1.1，若j>N则j＝1，i＝i+1，返回步骤5.1.1。

Images