CN108645398A - 一种基于结构化环境的即时定位与地图构建方法及系统 - Google Patents

Abstract

本发明公开了一种基于结构化环境的即时定位与地图构建方法及系统，方法包括：S1、通过相机对结构化环境中的多个位置进行拍摄，得到多个图像，每个位置对应一个图像；S2、对多个图像分别进行线特征提取，对多个图像中每个图像提取到的线条进行灭点提取；S3、对图像P_i的绝对旋转矩阵R_i进行优化，得到优化后的绝对旋转矩阵S4、根据图像P_i与图像P_j之间的相对旋转矩阵R_ij对绝对旋转矩阵进行校正；S5、计算图像P_i与图像P_j之间的相对平移量S6、根据相对平移量对图像P_i的绝对平移量t_i进行优化；S7、根据重新映射后的绝对旋转矩阵R_i和优化后的绝对平移量t_i，构建结构地图。本发明的有益效果是：充分利用结构环境所提供的信息，保证了高准确性。

Description

一种基于结构化环境的即时定位与地图构建方法及系统

技术领域

本发明涉及即时定位与地图构建技术领域，特别涉及一种基于结构化环境的即时定位与地图构建方法及系统。

背景技术

视觉测量是即时定位与地图构建技术SLAM中的重要研究课题之一。近年来，视觉测量已经被充分研究并广泛应用于机器人，自主驾驶，可穿戴计算和增强现实，并且在一些例如全球定位系统无效的室内和水下环境的特别情况下尤其适用。其中单目视觉测量使用单目摄像机的视觉光度仪具有更广阔的应用前景，更便宜，更紧凑，更容易校准等的优点。单目视觉测量的标准程序是首先提取一组图像特征并在连续帧中匹配它们，然后重建增量3D结构和最佳相机位姿，最后通过重投影误差最小化改善结构和位姿。因此，研究相机位姿及三维地图的估计和优化，对即时定位与地图构建成果有着重要意义。

一般来说，基于所使用特征的类型，现有的相机位姿及三维地图优化算法可以被分为两类：基于“非结构化信息”的估计及优化，以及基于“结构化信息”的相机位姿估计。

基于非结构化信息的视觉SLAM研究相对成熟。经典的相机位姿估计依赖于三维空间点到二维影像平面点形成的对应，由于特征点可以容易地检测并且在许多环境中具有良好的效率和性能，因此可利用这一特征建立三维空间中的点与二维相片中的点的对应并以此来估计相机的位姿。该方法存在的问题是当系统包含大量观测值时，这些方法就无法应用或变得十分低效，而且在低纹理场景特别是人造环境中效果很差。基于线的相机位姿估计应用做不到真正的点线结合，而且难以扩展用于冗余观察。

相比于非结构化场景，结构化场景，特别是人造环境，更容易提供规则的几何信息。室内环境通常带有结构化特点，例如墙壁、桌子、门窗和箱子等等，它们都带有显著的几何特征，适于作为环境特征。它们的优点是容易提取、相对稳定。现有方法往往利用墙线以及墙角等点、线作为特征。通过这些相对稳定的特征来对相机位置、姿态以及三维环境进行有效约束。然而，目前基于结构化信息的SLAM大都是二维SLAM，对三维复杂场景的研究还十分有限。一方面，这些方法的主要缺点在于在特征较少时精度难以保证。另一方面，一些针对极端场景，即特征十分匮乏，而设计的算法只能根据不同情况使用特定的模型，它们的普适性往往较差。

发明内容

本发明提供了一种基于结构化环境的即时定位与地图构建方法及系统，解决了现有技术中精度难以保证和普适性较差的技术问题。

本发明解决上述技术问题的技术方案如下：

一种基于结构化环境的即时定位与地图构建方法，包括：

S1、通过相机对结构化环境中的多个位置进行拍摄，得到多个图像，每个位置对应一个图像；

S2、对所述多个图像分别进行线特征提取，对所述多个图像中每个图像提取到的线条进行灭点提取，根据提取结果依次判断每个图像是否符合预设条件，获取第一个符合预设条件的图像后停止判断，将所述第一个符合预设条件的图像作为第一个图像P₁，得到所述第一个图像P₁与所述多个图像中未判断的图像组成的图像集i和S为正整数，所述符合预设条件的图像为提取到的灭点个数达到预设个数的图像；

S3、从图像集P中选取第i个图像P_i和第j个图像P_j，对所述图像P_i的绝对旋转矩阵R_i进行优化，得到优化后的绝对旋转矩阵R_i表示所述图像P_i相对于世界坐标系的绝对旋转矩阵，j为正整数；

S4、根据所述图像P_i与所述图像P_j之间的相对旋转矩阵R_ij对绝对旋转矩阵进行校正，将校正后的重新映射到R_i的集合{R_i}中，R_j表示所述图像P_j相对于世界坐标系的绝对旋转矩阵；

S5、计算所述图像P_i与所述图像P_j之间的相对平移量

S6、根据所述相对平移量对所述图像P_i的绝对平移量t_i进行优化，得到优化后的绝对平移量t_i；

S7、根据重新映射后的绝对旋转矩阵R_i和所述优化后的绝对平移量t_i，构建结构地图，所述结构地图由所述结构化环境中的多个3D结构线条组成，基于3D平面上的3D结构线条之间的平行特征，分别对3D平面和3D结构线条的位置进行控制，对所述结构地图进行优化，得到优化后的结构地图。

本发明的有益效果是：通过线特征提取和灭点提取，对拍摄得到的多个图像进行筛选，去除不符合预设条件的图像，提高准确性；利用两个图像之间的相对旋转矩阵去优化绝对旋转矩阵，计算两个图像之间的相对平移量，利用相对平移量去优化绝对平移量，提高即时定位和后续制图的准确性和鲁棒性，保证了普适性；充分利用结构环境所提供的信息，保证了高准确性。

在上述技术方案的基础上，本发明还可以做如下改进。

优选地，在所述S2之后，所述S3之前，还包括：

对所述图像P₁进行主方向初始化。

优选地，所述S3具体包括：

从图像集P中选取第i个图像P_i和第j个图像P_j；

建立目标函数E(w_i)，通过李代数对E(w_i)进行最小化；

通过Levenberg-Marquardt梯度下降方法获取最小化后的E(w_i)的最小值，将R_i作为初始值，对E(w_i)进行迭代收敛，得到优化后的绝对旋转矩阵

优选地，所述S5具体包括：

对图像集P中属于同一灭点的线段集合进行共面特征对的识别，得到多个共面点对和多个共面线对，根据映射后的绝对旋转矩阵R_i和R_j得到校正后的相对旋转矩阵R_ij，根据所述校正后的相对旋转矩阵R_ij分别构建共面点对观测方程和共面线对观测方程；

将所述共面点对观测方程和所述共面线对观测方程合并到统一模型中，基于所述统一模型构造超定线性系统通过最小二乘法计算得到其中，A表示系数矩阵，b表示非齐次项，由相机参数组成。

优选地，所述S7具体包括：

S71、根据所述优化后的绝对平移量t_i，构建结构地图，所述结构地图由所述结构化环境中的多个3D结构线条组成，将第m条3D结构线条L_m表示为Plucker坐标矩阵，将L_m对应的Plucker坐标表示为将L_m所在的3D平面表示为其中，η_m表示由L_m和世界坐标系W的原点O_w所构成的平面的法向量，λ_m表示L_m的单位方向向量，R^4×4表示4*4矩阵，n_π表示π的单位法向量，d_π表示O_w到π的垂直距离，m为正整数；

S72、基于3D平面上的3D结构线条之间的平行特征，根据d_c分别确定L_m中的λ_m和π中的n_π，将L_m表示为系数k_m和d_π的函数，通过d_π控制π的位置，通过k_m控制L_m的位置，d_c表示在世界坐标系W中所述相机到π的距离；

S73、对所述结构地图中的每个3D平面上的每条3D结构线条进行优化，得到优化后的结构地图。

优选地，所述S73具体包括：

将3D结构线条L_m从矩阵变换成一维向量u_m，通过直线运动矩阵M_i将u_m的坐标从世界坐标系W改变为相机坐标系C_i，得到L_m对应的Plucker坐标M_i由已知的R_i和t_i组成，M_i表示第i个图像对应的直线运动矩阵，C_i表示以第i个图像对应的位置为坐标原点的相机坐标系；

将投影到图像集获取最小化重投影误差，每条3D结构线条均对应S个图像；

构建基于所述最小化重投影误差的目标函数其中，表示第m条3D结构线条投影到图像集中第i个图像上的线段，和为线段的两个端点，表示端点到线段的投影线的距离，表示端点到线段的投影线的距离，M表示3D平面π上3D结构线条的条数，M为正整数；

通过最小化该目标函数的方式确定最优的系数k_m和最优的d_π，根据所述最优的系数k_m和所述最优的d_π对3D结构线条L_m进行优化，得到优化后的结构地图。

一种基于结构化环境的即时定位与地图构建系统，包括：

拍摄模块，用于通过相机对结构化环境中的多个位置进行拍摄，得到多个图像，每个位置对应一个图像；

提取模块，用于对所述多个图像分别进行线特征提取，对所述多个图像中每个图像提取到的线条进行灭点提取，根据提取结果依次判断每个图像是否符合预设条件，获取第一个符合预设条件的图像后停止判断，将所述第一个符合预设条件的图像作为第一个图像P₁，得到所述第一个图像P₁与所述多个图像中未判断的图像组成的图像集i和S为正整数，所述符合预设条件的图像为提取到的灭点个数达到预设个数的图像；

第一优化模块，用于从图像集P中选取第i个图像P_i和第j个图像P_j，对所述图像P_i的绝对旋转矩阵R_i进行优化，得到优化后的绝对旋转矩阵R_i表示所述图像P_i相对于世界坐标系的绝对旋转矩阵，j为正整数；

校正模块，用于根据所述图像P_i与所述图像P_j之间的相对旋转矩阵R_ij对绝对旋转矩阵进行校正，将校正后的重新映射到R_i的集合{R_i}中，R_j表示所述图像P_j相对于世界坐标系的绝对旋转矩阵；

计算模块，用于计算所述图像P_i与所述图像P_j之间的相对平移量

第二优化模块，用于根据所述相对平移量对所述图像P_i的绝对平移量t_i进行优化，得到优化后的绝对平移量t_i；

构建模块，用于根据重新映射后的绝对旋转矩阵R_i和所述优化后的绝对平移量t_i，构建结构地图，所述结构地图由所述结构化环境中的多个3D结构线条组成，基于3D平面上的3D结构线条之间的平行特征，分别对3D平面和3D结构线条的位置进行控制，对所述结构地图进行优化，得到优化后的结构地图。

优选地，还包括：

初始化模块，用于对所述图像P₁进行主方向初始化。

优选地，所述第一优化模块具体用于：

从图像集P中选取第i个图像P_i和第j个图像P_j；

建立目标函数E(w_i)，通过李代数对E(w_i)进行最小化；

通过Levenberg-Marquardt梯度下降方法获取最小化后的E(w_i)的最小值，将R_i作为初始值，对E(w_i)进行迭代收敛，得到优化后的绝对旋转矩阵

所述计算模块具体用于：

对图像集P中属于同一灭点的线段集合进行共面特征对的识别，得到多个共面点对和多个共面线对，根据映射后的绝对旋转矩阵R_i和R_j得到校正后的相对旋转矩阵R_ij，根据所述校正后的相对旋转矩阵R_ij分别构建共面点对观测方程和共面线对观测方程；

将所述共面点对观测方程和所述共面线对观测方程合并到统一模型中，基于所述统一模型构造超定线性系统通过最小二乘法计算得到其中，A表示系数矩阵，b表示非齐次项，由相机参数组成。

优选地，所述构建模块具体包括：

构建子模块，用于根据所述优化后的绝对平移量t_i，构建结构地图，所述结构地图由所述结构化环境中的多个3D结构线条组成，将第m条3D结构线条L_m表示为Plucker坐标矩阵，将L_m对应的Plucker坐标表示为将L_m所在的3D平面表示为其中，η_m表示由L_m和世界坐标系W的原点O_w所构成的平面的法向量，λ_m表示L_m的单位方向向量，R^4×4表示4*4矩阵，n_π表示π的单位法向量，d_π表示O_w到π的垂直距离，m为正整数；

控制子模块，用于基于3D平面上的3D结构线条之间的平行特征，根据d_c分别确定L_m中的λ_m和π中的n_π，将L_m表示为系数k_m和d_π的函数，通过d_π控制π的位置，通过k_m控制L_m的位置，d_c表示在世界坐标系W中所述相机到π的距离；

优化子模块，用于对所述结构地图中的每个3D平面上的每条3D结构线条进行优化，得到优化后的结构地图；

所述优化子模块具体用于：

将3D结构线条L_m从矩阵变换成一维向量u_m，通过直线运动矩阵M_i将u_m的坐标从世界坐标系W改变为相机坐标系C_i，得到L_m对应的Plucker坐标M_i由已知的R_i和t_i组成，M_i表示第i个图像对应的直线运动矩阵，C_i表示以第i个图像对应的位置为坐标原点的相机坐标系；

将投影到图像集获取最小化重投影误差，每条3D结构线条均对应S个图像；

构建基于所述最小化重投影误差的目标函数其中，表示第m条3D结构线条投影到图像集中第i个图像上的线段，和为线段的两个端点，表示端点到线段的投影线的距离，表示端点到线段的投影线的距离，M表示3D平面π上3D结构线条的条数，M为正整数；

通过最小化该目标函数的方式确定最优的系数k_m和最优的d_π，根据所述最优的系数k_m和所述最优的d_π对3D结构线条L_m进行优化，得到优化后的结构地图。

附图说明

图1为本发明实施例提供的一种基于结构化环境的即时定位与地图构建方法的流程示意图；

图2为本发明另一实施例提供的一种基于结构化环境的即时定位与地图构建系统的结构示意图；

图3为本发明另一实施例提供的一种基于结构化环境的即时定位与地图构建系统的结构示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

如图1所示，一种基于结构化环境的即时定位与地图构建方法，包括：

S1、通过相机对结构化环境中的多个位置进行拍摄，得到多个图像，每个位置对应一个图像；

S2、对多个图像分别进行线特征提取，对多个图像中每个图像提取到的线条进行灭点提取，根据提取结果依次判断每个图像是否符合预设条件，获取第一个符合预设条件的图像后停止判断，将第一个符合预设条件的图像作为第一个图像P₁，得到第一个图像P₁与多个图像中未判断的图像组成的图像集i和S为正整数，符合预设条件的图像为提取到的灭点个数达到预设个数的图像；

S3、从图像集P中选取第i个图像P_i和第j个图像P_j，对第i个图像P_i的绝对旋转矩阵R_i进行优化，得到优化后的绝对旋转矩阵R_i表示第i个图像P_i相对于世界坐标系的绝对旋转矩阵，j为正整数；

S4、根据第i个图像P_i与第j个图像P_j之间的相对旋转矩阵R_ij对绝对旋转矩阵进行校正，将校正后的重新映射到R_i的集合{R_i}中，R_j表示第j个图像P_j相对于世界坐标系的绝对旋转矩阵；

S5、计算第i个图像P_i与第j个图像P_j之间的相对平移量

S6、根据相对平移量对第i个图像P_i的绝对平移量t_i进行优化，得到优化后的绝对平移量t_i；

S7、根据重新映射后的绝对旋转矩阵R_i和优化后的绝对平移量t_i，构建结构地图，结构地图由结构化环境中的多个3D结构线条组成，基于3D平面上的3D结构线条之间的平行特征，分别对3D平面和3D结构线条的位置进行控制，对结构地图进行优化，得到优化后的结构地图。

将一台相机放在不同位置，且以不同角度/方向拍摄，一个位置拍一张图片，位置与图像一一对应。对所拍摄图片进行线特征提取，并基于所得线条进行灭点提取，若被提取的灭点少于两个，则舍弃该幅影像并尝试下一幅影像，直到发现第一个满足初始化要求的影像为止，即第一个图像P₁。

利用两幅影像之间的相对旋转矩阵R_ij来校正绝对旋转矩阵通过四元数参数化R_ij，并利用二次方程式进行改写，使用基来解算方程组，并通过自动生成器进行解算；再通过第一级逼近将李群映射到李代数中：ω_ij＝ω_j-ω_i＝[…-I…I…]ω＝Eω，其中，ω_ij是由已知R_ij的映射得到，ω包括所有待解算的未知数{ω_i}(由初始化)，和由两个单位矩阵I、多个空矩阵组成的E。通过结合所有的观测数{ω_ij}，建立一个基于上式的超定线性方程组，并通过优化器去计算理想的{ω_i}结果，将其重新映射到{R_i}中。

通过局部的归一化相对平移量来优化绝对平移量{t_i}，具体如下。

首先得到每个的尺度因子λ_ij，并利用公式计算t_ij，对于图像对{P_i,P_j}，使用来描述绝对平移量t_i和相对平移量t_ij之间的关系；再构造一个如Ht＝g的超定的线性系统，其中H是一个稀疏矩阵，g是相机参数，t是t_i的一个集合，此后再次使用优化器求解，得到最优的{t_i}。

通过线特征提取和灭点提取，对拍摄得到的多个图像进行筛选，去除不符合预设条件的图像，提高准确性；利用两个图像之间的相对旋转矩阵去优化绝对旋转矩阵，计算两个图像之间的相对平移量，利用相对平移量去优化绝对平移量，提高即时定位和后续制图的准确性和鲁棒性，保证了普适性；充分利用结构环境所提供的信息，保证了高准确性。

优选地，在S2之后，S3之前，还包括：

通过公式对图像P₁进行主方向初始化，其中，d_k表示图像P₁的第k个主方向向量，k∈{1,2,3}，I表示3*3单位矩阵，K为内参矩阵，表示图像P₁的第k个主方向坐标。

找到第一个图像P₁，然后对图像P₁进行主方向初始化，图像P₁具有三个主方向，三个主方向关联三个灭点，k取值1,2,3。

优选地，S3具体包括：

从图像集P中选取第i个图像P_i和第j个图像P_j；

建立目标函数E(w_i)，通过李代数对E(w_i)进行最小化，其中，w_i表示R_i在李群中的映射；

通过Levenberg-Marquardt梯度下降方法获取最小化后的E(w_i)的最小值，将R_i作为初始值，对E(w_i)进行迭代收敛，得到优化后的绝对旋转矩阵

基于约束建立目标函数，并利用李代数使其最小化：其中，和d_k都是归一化单元向量，李代数中w_i的是R_i在李群中的映射；为求解上述目标函数，应用Levenberg-Marquardt梯度下降方法去寻找E(w_i)的最小值，其中需要用到雅克比矩阵并将粗略的R_i结果作为梯度下降方法的初始值，通过迭代收敛得到优化后的

优选地，S5具体包括：

对图像集P中属于同一灭点的线段集合进行共面特征对的识别，得到多个共面点对和多个共面线对，根据映射后的绝对旋转矩阵R_i和R_j得到校正后的相对旋转矩阵R_ij，根据校正后的相对旋转矩阵R_ij分别构建共面点对观测方程和共面线对观测方程；

将共面点对观测方程和共面线对观测方程合并到统一模型中，基于统一模型构造超定线性系统通过最小二乘法计算得到其中，A表示系数矩阵，b表示非齐次项，由相机参数组成。

通过匹配相片I_i(相当于图像P_i)和I_j(相当于图像P_j)之间的线来获得对应的线集合，若相片I_i上的线集合的线和属于相片I_j上的线集合的线是匹配的，则线集合和是关联的，线和线是共面线对；之后基于共面线段在摄影基线上对应同一个“特征线”(characteristic line)的性质，获得了最终的共面匹配线，即共面线对。

基于一种等价地处理共面点和线的统一模型来计算归一化的相对平移量，具体如下。

共面点线所在空间平面π的单应矩阵H_ij，可以分解为其中，K为本质矩阵，d_i是从相机中心O_i到平面π的垂直距离，n_i是C_i中平面π的单位向量，t_ij/d_i是归一化的平移向量并记作以此为基础，对每一个在匹配线中上的点都可以得到关于匹配线的公式其中，基于优化后的R_i，精确的R_ij可以用来确定，n_i可以用计算的进行分别确定。

相似的，得到关于共面点对的公式由于的秩是2，该式可以被重写成两个关于的线性等式；之后合并这两种观测方程到一个统一的模型中，构造如下形式的一个超定的线性系统：使用最小二乘求解A表示系数矩阵，b表示非齐次项，由相机参数组成。

优选地，S7具体包括：

S71、根据根据重新映射后的绝对旋转矩阵R_i和优化后的绝对平移量t_i，构建结构地图，结构地图由结构化环境中的多个3D结构线条组成，将第m条3D结构线条L_m表示为Plucker坐标矩阵，将L_m对应的Plucker坐标表示为将L_m所在的3D平面表示为其中，η_m表示由L_m和世界坐标系W的原点O_w所构成的平面的法向量，λ_m表示L_m的单位方向向量，R^4×4表示4*4矩阵，n_π表示π的单位法向量，d_π表示O_w到π的垂直距离，m为正整数；

S72、基于3D平面上的3D结构线条之间的平行特征，根据d_c分别确定L_m中的λ_m和π中的n_π，将L_m表示为系数k_m和d_π的函数，通过d_π控制π的位置，通过k_m控制L_m的位置，d_c表示在世界坐标系W中相机到π的距离；

S73、对结构地图中的每个3D平面上的每条3D结构线条进行优化，得到优化后的结构地图。

使用一种由3D的结构线条构成的“结构地图”来表示3D环境，具体如下。

用Plucker矩阵来表示3D线条L_m，即其中η_m是由L_m和W的原点O_w所构成的平面的法向量，λ_m是单位方向向量，相应的Plucker坐标用表示；此外，一个3D平面用来表示，这里n_π是π的单位法向量，d_π是O_w到π的垂直距离。

利用结构化环境中的平行约束，根据相应的d_k分别分别确定L_m中的λ_m和π中的n_π，改变相对于η_m的线性系统的共面性约束；基于L_m的自由度是2，少于自由度为3的η_m，可知η_m的一般解中有一个未知的系数k_m和不确定的d_π，则每一个平面π上的3D线L_m被表示成一个关于其对应的系数k_m和常数d_π的函数。

对于平面π，我们分别确定了它的法向量n_π，所以它只能沿着n_π移动，其移动由未确定的d_π来反映；由于L_m限制在平面π上，并且它的λ_m是分别确定的，所以它只能在平面π上顺着λ_m的垂直方向移动，这个移动由系数k_m来体现。

优选地，S73具体包括：

将3D结构线条L_m从矩阵变换成一维向量u_m，通过直线运动矩阵M_i将u_m的坐标从世界坐标系W改变为相机坐标系C_i，得到L_m对应的Plucker坐标M_i由已知的R_i和t_i组成，M_i表示第i个图像对应的直线运动矩阵，C_i表示以第i个图像对应的位置为坐标原点的相机坐标系；

将投影到图像集获取最小化重投影误差，每条3D结构线条均对应S个图像；

构建基于最小化重投影误差的目标函数其中，表示第m条3D结构线条投影到图像集中第i个图像上的线段，和为线段的两个端点，表示端点到线段的投影线的距离，表示端点到线段的投影线的距离，M表示3D平面π上3D结构线条的条数，M为正整数；

通过最小化该目标函数的方式确定最优的系数k_m和最优的d_π，根据最优的系数k_m和最优的d_π对3D结构线条L_m进行优化，得到优化后的结构地图。

如图2所示，一种基于结构化环境的即时定位与地图构建系统，包括：

拍摄模块1，用于通过相机对结构化环境中的多个位置进行拍摄，得到多个图像，每个位置对应一个图像；

提取模块2，用于对多个图像分别进行线特征提取，对多个图像中每个图像提取到的线条进行灭点提取，根据提取结果依次判断每个图像是否符合预设条件，获取第一个符合预设条件的图像后停止判断，将第一个符合预设条件的图像作为第一个图像P₁，得到第一个图像P₁与多个图像中未判断的图像组成的图像集i和S为正整数，符合预设条件的图像为提取到的灭点个数达到预设个数的图像；

第一优化模块3，用于从图像集P中选取第i个图像P_i和第j个图像P_j，对图像P_i的绝对旋转矩阵R_i进行优化，得到优化后的绝对旋转矩阵R_i表示图像P_i相对于世界坐标系的绝对旋转矩阵，j为正整数；

校正模块4，用于根据图像P_i与图像P_j之间的相对旋转矩阵R_ij对绝对旋转矩阵进行校正，将校正后的重新映射到R_i的集合{R_i}中，R_j表示图像P_j相对于世界坐标系的绝对旋转矩阵；

计算模块5，用于计算图像P_i与图像P_j之间的相对平移量

第二优化模块6，用于根据相对平移量对图像P_i的绝对平移量t_i进行优化，得到优化后的绝对平移量t_i；

构建模块7，用于根据重新映射后的绝对旋转矩阵R_i和优化后的绝对平移量t_i，构建结构地图，结构地图由结构化环境中的多个3D结构线条组成，基于3D平面上的3D结构线条之间的平行特征，分别对3D平面和3D结构线条的位置进行控制，对结构地图进行优化，得到优化后的结构地图。

如图3所示，还包括：

初始化模块8，用于通过公式对图像P₁进行主方向初始化，其中，d_k表示图像P₁的第k个主方向向量，k∈{1,2,3}，I表示3*3单位矩阵，K为内参矩阵，表示图像P₁的第k个主方向坐标。

优选地，第一优化模块3具体用于：

从图像集P中选取第i个图像P_i和第j个图像P_j；

建立目标函数E(w_i)，通过李代数对E(w_i)进行最小化，其中，w_i表示R_i在李群中的映射；

通过Levenberg-Marquardt梯度下降方法获取最小化后的E(w_i)的最小值，将R_i作为初始值，对E(w_i)进行迭代收敛，得到优化后的绝对旋转矩阵

计算模块5具体用于：

对图像集P中属于同一灭点的线段集合进行共面特征对的识别，得到多个共面点对和多个共面线对，根据映射后的绝对旋转矩阵R_i和R_j得到校正后的相对旋转矩阵R_ij，根据校正后的相对旋转矩阵R_ij分别构建共面点对观测方程和共面线对观测方程；

将共面点对观测方程和共面线对观测方程合并到统一模型中，基于统一模型构造超定线性系统通过最小二乘法计算得到其中，A表示系数矩阵，b表示非齐次项，由相机参数组成。

优选地，构建模块7具体包括：

构建子模块，用于根据重新映射后的绝对旋转矩阵R_i和优化后的绝对平移量t_i，构建结构地图，结构地图由结构化环境中的多个3D结构线条组成，将第m条3D结构线条L_m表示为Plucker坐标矩阵，将L_m对应的Plucker坐标表示为将L_m所在的3D平面表示为其中，η_m表示由L_m和世界坐标系W的原点O_w所构成的平面的法向量，λ_m表示L_m的单位方向向量，R^4×4表示4*4矩阵，n_π表示π的单位法向量，d_π表示O_w到π的垂直距离，m为正整数；

控制子模块，用于基于3D平面上的3D结构线条之间的平行特征，根据d_c分别确定L_m中的λ_m和π中的n_π，将L_m表示为系数k_m和d_π的函数，通过d_π控制π的位置，通过k_m控制L_m的位置，d_c表示在世界坐标系W中相机到π的距离；

优化子模块，用于对结构地图中的每个3D平面上的每条3D结构线条进行优化，得到优化后的结构地图；

优化子模块具体用于：

将3D结构线条L_m从矩阵变换成一维向量u_m，通过直线运动矩阵M_i将u_m的坐标从世界坐标系W改变为相机坐标系C_i，得到L_m对应的Plucker坐标M_i由已知的R_i和t_i组成，M_i表示第i个图像对应的直线运动矩阵，C_i表示以第i个图像对应的位置为坐标原点的相机坐标系；

将投影到图像集获取最小化重投影误差，每条3D结构线条均对应S个图像；

构建基于最小化重投影误差的目标函数其中，表示第m条3D结构线条投影到图像集中第i个图像上的线段，和为线段的两个端点，表示端点到线段的投影线的距离，表示端点到线段的投影线的距离，M表示3D平面π上3D结构线条的条数，M为正整数；

通过最小化该目标函数的方式确定最优的系数k_m和最优的d_π，根据最优的系数k_m和最优的d_π对3D结构线条L_m进行优化，得到优化后的结构地图。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于结构化环境的即时定位与地图构建方法，其特征在于，包括：

S1、通过相机对结构化环境中的多个位置进行拍摄，得到多个图像，每个位置对应一个图像；

S2、对所述多个图像分别进行线特征提取，对所述多个图像中每个图像提取到的线条进行灭点提取，根据提取结果依次判断每个图像是否符合预设条件，获取第一个符合预设条件的图像后停止判断，将所述第一个符合预设条件的图像作为第一个图像P₁，得到所述第一个图像P₁与所述多个图像中未判断的图像组成的图像集i和S为正整数，所述符合预设条件的图像为提取到的灭点个数达到预设个数的图像；

S3、从图像集P中选取第i个图像P_i和第j个图像P_j，对所述图像P_i的绝对旋转矩阵R_i进行优化，得到优化后的绝对旋转矩阵R_i表示所述图像P_i相对于世界坐标系的绝对旋转矩阵，j为正整数；

S4、根据所述图像P_i与所述图像P_j之间的相对旋转矩阵R_ij对绝对旋转矩阵进行校正，将校正后的重新映射到R_i的集合{R_i}中，R_j表示所述图像P_j相对于世界坐标系的绝对旋转矩阵；

S5、计算所述图像P_i与所述图像P_j之间的相对平移量

S6、根据所述相对平移量对所述图像P_i的绝对平移量t_i进行优化，得到优化后的绝对平移量t_i；

S7、根据重新映射后的绝对旋转矩阵R_i和所述优化后的绝对平移量t_i，构建结构地图，所述结构地图由所述结构化环境中的多个3D结构线条组成，基于3D平面上的3D结构线条之间的平行特征，分别对3D平面和3D结构线条的位置进行控制，对所述结构地图进行优化，得到优化后的结构地图。

2.根据权利要求1所述的一种基于结构化环境的即时定位与地图构建方法，其特征在于，在所述S2之后，所述S3之前，还包括：

对所述图像P₁进行主方向初始化。

3.根据权利要求2所述的一种基于结构化环境的即时定位与地图构建方法，其特征在于，所述S3具体包括：

从图像集P中选取第i个图像P_i和第j个图像P_j；

建立目标函数E(w_i)，通过李代数对E(w_i)进行最小化；

通过Levenberg-Marquardt梯度下降方法获取最小化后的E(w_i)的最小值，将R_i作为初始值，对E(w_i)进行迭代收敛，得到优化后的绝对旋转矩阵

4.根据权利要求3所述的一种基于结构化环境的即时定位与地图构建方法，其特征在于，所述S5具体包括：

对图像集P中属于同一灭点的线段集合进行共面特征对的识别，得到多个共面点对和多个共面线对，根据映射后的绝对旋转矩阵R_i和R_j得到校正后的相对旋转矩阵R_ij，根据所述校正后的相对旋转矩阵R_ij分别构建共面点对观测方程和共面线对观测方程；

将所述共面点对观测方程和所述共面线对观测方程合并到统一模型中，基于所述统一模型构造超定线性系统通过最小二乘法计算得到其中，A表示系数矩阵，b表示非齐次项，由相机参数组成。

5.根据权利要求2-4任一项所述的一种基于结构化环境的即时定位与地图构建方法，其特征在于，所述S7具体包括：

S71、根据所述优化后的绝对平移量t_i，构建结构地图，所述结构地图由所述结构化环境中的多个3D结构线条组成，将第m条3D结构线条L_m表示为Plucker坐标矩阵，将L_m对应的Plucker坐标表示为将L_m所在的3D平面表示为其中，η_m表示由L_m和世界坐标系W的原点O_w所构成的平面的法向量，λ_m表示L_m的单位方向向量，R^4×4表示4*4矩阵，n_π表示π的单位法向量，d_π表示O_w到π的垂直距离，m为正整数；

S72、基于3D平面上的3D结构线条之间的平行特征，根据d_c分别确定L_m中的λ_m和π中的n_π，将L_m表示为系数k_m和d_π的函数，通过d_π控制π的位置，通过k_m控制L_m的位置，d_c表示在世界坐标系W中所述相机到π的距离；

S73、对所述结构地图中的每个3D平面上的每条3D结构线条进行优化，得到优化后的结构地图。

6.根据权利要求5所述的一种基于结构化环境的即时定位与地图构建方法，其特征在于，所述S73具体包括：

将3D结构线条L_m从矩阵变换成一维向量u_m，通过直线运动矩阵M_i将u_m的坐标从世界坐标系W改变为相机坐标系C_i，得到L_m对应的Plucker坐标M_i由已知的R_i和t_i组成，M_i表示第i个图像对应的直线运动矩阵，C_i表示以第i个图像对应的位置为坐标原点的相机坐标系；

将投影到图像集获取最小化重投影误差，每条3D结构线条均对应S个图像；

构建基于所述最小化重投影误差的目标函数其中，表示第m条3D结构线条投影到图像集中第i个图像上的线段，和为线段的两个端点，表示端点到线段的投影线的距离，表示端点到线段的投影线的距离，M表示3D平面π上3D结构线条的条数，M为正整数；

通过最小化该目标函数的方式来确定最优的系数k_m和最优的d_π，根据所述最优的系数k_m和所述最优的d_π对3D结构线条L_m进行优化，得到优化后的结构地图。

7.一种基于结构化环境的即时定位与地图构建系统，其特征在于，包括：

拍摄模块，用于通过相机对结构化环境中的多个位置进行拍摄，得到多个图像，每个位置对应一个图像；

提取模块，用于对所述多个图像分别进行线特征提取，对所述多个图像中每个图像提取到的线条进行灭点提取，根据提取结果依次判断每个图像是否符合预设条件，获取第一个符合预设条件的图像后停止判断，将所述第一个符合预设条件的图像作为第一个图像P₁，得到所述第一个图像P₁与所述多个图像中未判断的图像组成的图像集i和S为正整数，所述符合预设条件的图像为提取到的灭点个数达到预设个数的图像；

第一优化模块，用于从图像集P中选取第i个图像P_i和第j个图像P_j，对所述图像P_i的绝对旋转矩阵R_i进行优化，得到优化后的绝对旋转矩阵R_i表示所述图像P_i相对于世界坐标系的绝对旋转矩阵，j为正整数；

校正模块，用于根据所述图像P_i与所述图像P_j之间的相对旋转矩阵R_ij对绝对旋转矩阵进行校正，将校正后的重新映射到R_i的集合{R_i}中，R_j表示所述图像P_j相对于世界坐标系的绝对旋转矩阵；

计算模块，用于计算所述图像P_i与所述图像P_j之间的相对平移量

第二优化模块，用于根据所述相对平移量对所述图像P_i的绝对平移量t_i进行优化，得到优化后的绝对平移量t_i；

构建模块，用于根据重新映射后的绝对旋转矩阵R_i和所述优化后的绝对平移量t_i，构建结构地图，所述结构地图由所述结构化环境中的多个3D结构线条组成，基于3D平面上的3D结构线条之间的平行特征，分别对3D平面和3D结构线条的位置进行控制，对所述结构地图进行优化，得到优化后的结构地图。

8.根据权利要求7所述的一种基于结构化环境的即时定位与地图构建系统，其特征在于，还包括：

初始化模块，用于对所述图像P₁进行主方向初始化。

9.根据权利要求8所述的一种基于结构化环境的即时定位与地图构建系统，其特征在于，所述第一优化模块具体用于：

从图像集P中选取第i个图像P_i和第j个图像P_j；

建立目标函数E(w_i)，通过李代数对E(w_i)进行最小化；

通过Levenberg-Marquardt梯度下降方法获取最小化后的E(w_i)的最小值，将R_i作为初始值，对E(w_i)进行迭代收敛，得到优化后的绝对旋转矩阵

所述计算模块具体用于：

对图像集P中属于同一灭点的线段集合进行共面特征对的识别，得到多个共面点对和多个共面线对，根据映射后的绝对旋转矩阵R_i和R_j得到校正后的相对旋转矩阵R_ij，根据所述校正后的相对旋转矩阵R_ij分别构建共面点对观测方程和共面线对观测方程；

将所述共面点对观测方程和所述共面线对观测方程合并到统一模型中，基于所述统一模型构造超定线性系统通过最小二乘法计算得到其中，A表示系数矩阵，b表示非齐次项，由相机参数组成。

10.根据权利要求8或9所述的一种基于结构化环境的即时定位与地图构建系统，其特征在于，所述构建模块具体包括：

构建子模块，用于根据所述优化后的绝对平移量t_i，构建结构地图，所述结构地图由所述结构化环境中的多个3D结构线条组成，将第m条3D结构线条L_m表示为Plucker坐标矩阵，将L_m对应的Plucker坐标表示为将L_m所在的3D平面表示为其中，η_m表示由L_m和世界坐标系W的原点O_w所构成的平面的法向量，λ_m表示L_m的单位方向向量，R^4×4表示4*4矩阵，n_π表示π的单位法向量，d_π表示O_w到π的垂直距离，m为正整数；

控制子模块，用于基于3D平面上的3D结构线条之间的平行特征，根据d_c分别确定L_m中的λ_m和π中的n_π，将L_m表示为系数k_m和d_π的函数，通过d_π控制π的位置，通过k_m控制L_m的位置，d_c表示在世界坐标系W中所述相机到π的距离；

优化子模块，用于对所述结构地图中的每个3D平面上的每条3D结构线条进行优化，得到优化后的结构地图；

所述优化子模块具体用于：

将3D结构线条L_m从矩阵变换成一维向量u_m，通过直线运动矩阵M_i将u_m的坐标从世界坐标系W改变为相机坐标系C_i，得到L_m对应的Plucker坐标M_i由已知的R_i和t_i组成，M_i表示第i个图像对应的直线运动矩阵，C_i表示以第i个图像对应的位置为坐标原点的相机坐标系；

将投影到图像集获取最小化重投影误差，每条3D结构线条均对应S个图像；

构建基于所述最小化重投影误差的目标函数其中，表示第m条3D结构线条投影到图像集中第i个图像上的线段，和为线段的两个端点，表示端点到线段的投影线的距离，表示端点到线段的投影线的距离，M表示3D平面π上3D结构线条的条数，M为正整数；

通过最小化该目标函数的方式确定最优的系数k_m和最优的d_π，根据所述最优的系数k_m和所述最优的d_π对3D结构线条L_m进行优化，得到优化后的结构地图。