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

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

背景技术：

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

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

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

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

技术实现要素：

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

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

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

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

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

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

s4、根据所述图像pi与所述图像pj之间的相对旋转矩阵rij对绝对旋转矩阵进行校正，将校正后的重新映射到ri的集合{ri}中，rj表示所述图像pj相对于世界坐标系的绝对旋转矩阵；

s5、计算所述图像pi与所述图像pj之间的相对平移量

s6、根据所述相对平移量对所述图像pi的绝对平移量ti进行优化，得到优化后的绝对平移量ti；

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

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

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

优选地，在所述s2之后，所述s3之前，还包括：

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

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

从图像集p中选取第i个图像pi和第j个图像pj；

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

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

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

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

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

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

s71、根据所述优化后的绝对平移量ti，构建结构地图，所述结构地图由所述结构化环境中的多个3d结构线条组成，将第m条3d结构线条lm表示为plucker坐标矩阵，将lm对应的plucker坐标表示为将lm所在的3d平面表示为其中，ηm表示由lm和世界坐标系w的原点ow所构成的平面的法向量，λm表示lm的单位方向向量，r^4×4表示4*4矩阵，nπ表示π的单位法向量，dπ表示ow到π的垂直距离，m为正整数；

s72、基于3d平面上的3d结构线条之间的平行特征，根据dc分别确定lm中的λm和π中的nπ，将lm表示为系数km和dπ的函数，通过dπ控制π的位置，通过km控制lm的位置，dc表示在世界坐标系w中所述相机到π的距离；

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

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

将3d结构线条lm从矩阵变换成一维向量um，通过直线运动矩阵mi将um的坐标从世界坐标系w改变为相机坐标系ci，得到lm对应的plucker坐标mi由已知的ri和ti组成，mi表示第i个图像对应的直线运动矩阵，ci表示以第i个图像对应的位置为坐标原点的相机坐标系；

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

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

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

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

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

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

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

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

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

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

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

优选地，还包括：

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

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

从图像集p中选取第i个图像pi和第j个图像pj；

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

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

所述计算模块具体用于：

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

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

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

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

控制子模块，用于基于3d平面上的3d结构线条之间的平行特征，根据dc分别确定lm中的λm和π中的nπ，将lm表示为系数km和dπ的函数，通过dπ控制π的位置，通过km控制lm的位置，dc表示在世界坐标系w中所述相机到π的距离；

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

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

将3d结构线条lm从矩阵变换成一维向量um，通过直线运动矩阵mi将um的坐标从世界坐标系w改变为相机坐标系ci，得到lm对应的plucker坐标mi由已知的ri和ti组成，mi表示第i个图像对应的直线运动矩阵，ci表示以第i个图像对应的位置为坐标原点的相机坐标系；

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

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

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

附图说明

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

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

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

具体实施方式

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

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

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

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

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

s4、根据第i个图像pi与第j个图像pj之间的相对旋转矩阵rij对绝对旋转矩阵进行校正，将校正后的重新映射到ri的集合{ri}中，rj表示第j个图像pj相对于世界坐标系的绝对旋转矩阵；

s5、计算第i个图像pi与第j个图像pj之间的相对平移量

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

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

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

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

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

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

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

优选地，在s2之后，s3之前，还包括：

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

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

优选地，s3具体包括：

从图像集p中选取第i个图像pi和第j个图像pj；

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

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

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

优选地，s5具体包括：

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

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

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

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

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

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

优选地，s7具体包括：

s71、根据根据重新映射后的绝对旋转矩阵ri和优化后的绝对平移量ti，构建结构地图，结构地图由结构化环境中的多个3d结构线条组成，将第m条3d结构线条lm表示为plucker坐标矩阵，将lm对应的plucker坐标表示为将lm所在的3d平面表示为其中，ηm表示由lm和世界坐标系w的原点ow所构成的平面的法向量，λm表示lm的单位方向向量，r^4×4表示4*4矩阵，nπ表示π的单位法向量，dπ表示ow到π的垂直距离，m为正整数；

s72、基于3d平面上的3d结构线条之间的平行特征，根据dc分别确定lm中的λm和π中的nπ，将lm表示为系数km和dπ的函数，通过dπ控制π的位置，通过km控制lm的位置，dc表示在世界坐标系w中相机到π的距离；

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

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

用plucker矩阵来表示3d线条lm，即其中ηm是由lm和w的原点ow所构成的平面的法向量，λm是单位方向向量，相应的plucker坐标用表示；此外，一个3d平面用来表示，这里nπ是π的单位法向量，dπ是ow到π的垂直距离。

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

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

优选地，s73具体包括：

将3d结构线条lm从矩阵变换成一维向量um，通过直线运动矩阵mi将um的坐标从世界坐标系w改变为相机坐标系ci，得到lm对应的plucker坐标mi由已知的ri和ti组成，mi表示第i个图像对应的直线运动矩阵，ci表示以第i个图像对应的位置为坐标原点的相机坐标系；

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

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

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

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

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

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

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

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

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

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

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

如图3所示，还包括：

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

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

从图像集p中选取第i个图像pi和第j个图像pj；

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

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

计算模块5具体用于：

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

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

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

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

控制子模块，用于基于3d平面上的3d结构线条之间的平行特征，根据dc分别确定lm中的λm和π中的nπ，将lm表示为系数km和dπ的函数，通过dπ控制π的位置，通过km控制lm的位置，dc表示在世界坐标系w中相机到π的距离；

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

优化子模块具体用于：

将3d结构线条lm从矩阵变换成一维向量um，通过直线运动矩阵mi将um的坐标从世界坐标系w改变为相机坐标系ci，得到lm对应的plucker坐标mi由已知的ri和ti组成，mi表示第i个图像对应的直线运动矩阵，ci表示以第i个图像对应的位置为坐标原点的相机坐标系；

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

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

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

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