CN111829522B - 即时定位与地图构建方法、计算机设备以及装置 - Google Patents

Abstract

本发明公开了即时定位与地图构建方法、计算机设备以及装置，该方法包括：获取一帧由可移动设备拍摄的双目图像序列，将其作为当前帧；计算所述当前帧与参考关键帧之间的第一融合约束，确定所述可移动设备的位姿；其中所述第一融合约束是由所述参考关键帧的所述双目图像序列中两图像之间的静态约束和所述当前帧与所述参考关键帧之间的动态约束组成的。通过上述方式，本发明能够在计算位姿的过程中，兼顾了静态约束和动态约束，使得所确定的位姿更加准确。

Description

即时定位与地图构建方法、计算机设备以及装置

技术领域

本发明涉及定位导航技术领域，特别是涉及一种即时定位与地图构建领域。

背景技术

SLAM(Simultaneous Localization and Mapping)，即同步定位与地图构建技术，可描述为智能设备放入未知环境中，并从一个未知位置开始移动，在移动过程中通过传感器获取周围环境的二维或三维空间结构的信息，同时根据位姿估计实现自身定位和周围环境的建图。视觉SLAM是在SLAM框架的基础上，利用视觉传感器(如单目，双目以及RGB-D深度相机等)实时的构建周围环境的增量式地图。

视觉SLAM方法包括视觉里程计、后端优化、闭环检测以及构建地图四个主要部分。其中视觉里程计是通过传感器输入原始图像序列粗略地估计出当前相机的位姿。视觉里程计的实现主要有2种方法：特征点法和直接法，其中，特征点法利用图像中的局部特征点进行特征匹配，通过匹配后的特征点建立几何约束条件，计算重投影误差来获得位姿估计；而直接法将数据关联与位姿估计放在一个统一的非线性优化问题中，无需特征匹配，通过搜索图像梯度，计算两个像素间的亮度误差来估计位姿。

在面向SLAM的研究中，大多数采用特征点法进行位姿的估计。本申请的发明人在长期的研发过程中，发现利用这些方法往往需要对特征点进行一一对应匹配，使得计算耗时并且复杂度高。在大规模室外环境中，只能依赖类似车辆，行人等明显的特征点，特征点匹配的准确性会对定位的效果起到严重的影响，使得位姿的估计鲁棒性不佳。

发明内容

本发明主要解决的技术问题是提供一种即时定位与地图构建方法，能够提高位姿估计的鲁棒性。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种即时定位与地图构建方法，获取一帧由可移动设备拍摄的双目图像序列，将其作为当前帧；计算所述当前帧与参考关键帧之间的第一融合约束，确定所述可移动设备的位姿；其中所述第一融合约束是由所述参考关键帧的所述双目图像序列中两图像之间的静态约束和所述当前帧与所述参考关键帧之间的动态约束组成的

为解决上述技术问题，本发明采用的另一个技术方案是：提供一种计算机设备，该计算机设备包括相互耦接的处理器和双目拍摄装置，所述双目拍摄装置用于拍摄双目图像，形成所述双目图像序列，所述处理器用于执行指令以实现上述的即时定位与地图构建方法。

为解决上述技术问题，本发明采用的另一个技术方案是：提供一种具有存储功能的装置，所述装置存储有程序，所述程序被执行时能够实现上述的即时定位与地图构建方法。

本发明的有益效果是：区别于现有技术的情况，本发明通过计算两图像之间的融合约束来确定光度误差并估计位姿，无需依赖于图像中具有明显特征的点即可进行位姿估计，适用的范围更加广泛，对环境的要求低。在计算位姿的过程中，兼顾了静态约束和动态约束，使得所确定的位姿更加准确。通过仅针对图像中关键点进行计算的方式，减少了运算量，提高了位姿估计的速度。

附图说明

图1是根据本申请一实施方式的即时定位与地图构建方法的流程示意图；

图2是根据本申请一实施方式中SLAM方法中的静态约束的计算方法的流程示意图；

图3是根据本申请一实施方式中双目图像序列中两图之间几何关系的示意图；

图4是根据本申请另一实施方式的SLAM方法的流程示意图；

图5是根据本申请一实施方式中SLAM方法中的闭环检测方法的流程示意图；

图6是根据本申请一实施方式中SLAM方法中的闭环检测方法的流程示意图；

图7是根据本申请一实施方式中计算机设备的结构示意图；

图8是根据本申请一实施方式中具有存储功能的装置的结构示意图。

具体实施方式

为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。

本发明的实施例可以应用于大规模室外环境下带有双目拍摄装置的可移动设备的SLAM方法。其中带有双目拍摄装置的可移动电子设备包括但不限于机器人，无人机或手持设备等。应当理解的是，本申请的系统及方法的应用场景仅仅是本申请的一些示例或实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图将本申请应用于其他类似情景。

参阅图1，图1是根据本申请一实施方式的即时定位与地图构建方法的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图1所示的流程顺序为限。如图1所示，该方法包括：

步骤S110：获取一帧由可移动设备拍摄的双目图像序列，将其作为当前帧。

在一实施方式中，双目图像序列可以是从双目摄像装置中直接获取的，也可以是从其他储存装置中调取的。其中，双目图像序列具有两张不同视角但包含全部或部分相同的图像特征的图像。双目摄像装置中的两个摄像头的间距是固定的，但是这两目摄像头的排列方式可以是水平的，也可以是垂直的或是以任何角度倾斜的。仅仅为了方便描述，下文中将这两张图像分别称为左图和右图。

在一实施方式中，可以对所获取的双目图像序列构建图像金字塔。图像金字塔是指一系列以金字塔形状排列的分辨率逐步降低，并且来源于同一张原始图的图像集合。在一实施方式中，图像金字塔的层数可以根据实际的应用场景确定。例如，图像金字塔的层数可以为5层，6层或7层。

步骤S120：计算当前帧与参考关键帧之间的第一融合约束，确定所述可移动设备的位姿；其中所述第一融合约束是由所述参考关键帧的所述双目图像序列中两个图像之间的静态约束和所述当前帧与所述参考关键帧之间的动态约束组成的。

其中，静态约束可以是指一帧双目图像序列中两个图像之间的约束；动态约束可以是指不同帧图像之间的约束。约束可以是指两图像之间光度误差的约束。

在一实施方式中，静态约束可以是指两图像中至少部分点与投影点之间光度误差与深度之间的相互约束条件。具体地，静态约束可以是指双目图像序列中的左图上的至少部分点与这些点在右图上的投影点之间的光度误差与位姿的相互约束条件。

在一实施方式中，动态约束可以是指两帧不同时刻的图像中至少部分点与投影点之间光度误差与位姿之间的相互约束条件。具体的，动态约束可以是指参考关键帧图像中至少部分点与这些点在当前帧图像上的投影点之间光度误差与位姿之间的相互约束条件。

在一实施方式中，静态/动态约束可以包括两图像中至少部分点与投影点之间的光度误差的能量函数以及光度误差的雅克比矩阵。具体的，静态约束中光度误差的能量函数可以表示为，如公式(1)所示；光度误差的雅克比矩阵可以表示为，如公式(2)所示：

p′＝Π(RΠ^-1(p,d)+t) (2)

其中

是静态光度误差，

是右图的点的光度，

是左图的点的光度，p是关键点，p′是投影点，T^RL是左右两个之间的位置关系，d为深度，c为相机内参，

均为光度参数，t为位移向量，R为旋转向量。静态约束的具体构建方法详见图2及其相关描述，在此不赘述。

在一实施方式中，可以计算参考关键帧的双目图像序列中的两张图像之间的静态约束直至收敛，确定所述参考关键帧中至少部分点的深度。在一实施方式中，至少部分点可以是指图像中的关键点。具体计算方法详见图2及其相关描述，在此不赘述。

在一实施方式中，可以基于所确定的参考关键帧中至少部分点的深度，计算当前帧与参考关键帧之间的动态约束直至收敛，从而确定所述可移动设备的位姿。

具体的，当前帧与参考关键帧之间动态约束中光度误差的能量函数如公式(3)所示，光度误差的雅克比矩阵如公式(4)所示：

p_j＝Π(RΠ^-1(p_i,d)+t) (4)

其中，

为动态光度误差，I_i为参考关键帧中点的光度、I_j为当前帧中投影点的光度，p_i为关键点，p_j为投影点，T_ji为位姿，d为深度，c为相机内参，a_ia_jbibj均为光度参数，t为位移向量，R为旋转向量。

设定参考关键帧中至少部分点投影在当前帧上深度不变，即被投影点与投影点的深度d相同。通过使用高斯牛顿法，以由粗到精的方式，计算动态约束直至收敛，确定最小光度误差，并以最小光度误差所对应的位姿为当前帧的位姿，即公式(3)中的T_ji。

该实施方式中，通过计算两图像之间的融合约束来确定光度误差并估计位姿，无需依赖于图像中具有明显特征的点即可进行位姿估计，适用的范围更加广泛，对环境的要求低。在计算位姿的过程中，兼顾了静态约束和动态约束，使得所确定的位姿更加准确。通过仅针对图像中关键点进行计算的方式，减少了运算量，提高了位姿估计的速度。

参阅图2，图2是根据本申请一实施方式中SLAM方法中的静态约束的计算方法的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图2所示的流程顺序为限。如图2所示，本实施例包括：

步骤S210：确定图像上的关键点。

在一实施方式中，可以确定图像中的关键点。其中关键点的确定方法包括，首先，将图像进行网格划分，确定需要每一格中提取关键点的数量。然后再通过对图像进行搜索，提取每一格中像素大于阈值的点，将其作为关键点。其中，初始搜索步长为预设值，判断利用所述初始搜索步长是否能够获得足够数量的关键点，若为是，则停止搜索；若为否，则扩大搜索步长继续搜索。在一实施方式中，每一格中的阈值可以是统一基于整张图像确定的，也可以是基于每一格中的图像确定的自适应阈值。采用该方法可以使得在图像中提取的关键点的分布较为均匀，使得后续的位姿估计可以全面的参考全部图像，更加鲁棒。

步骤S220：基于所述两图像之间的几何关系，确定关键点在另一个图像中的投影点的获取范围。

由于双目摄像设备中，两目摄像头的间距是固定的，利用这一特性可以建立在同一帧时刻下左、右图像之间的几何关系。下面以一个点为例进行详细介绍。

如图3所示，图3是根据本申请一实施方式中双目图像序列中两图之间几何关系的示意图。环境中的3D点P在左图视角下处于射线O₁P₁上，但是其空间位置不能确定。运用极线搜索技术，右图中e₂p₂的连线就是空间点P在右图中可能出现的投影位置，即e₂p₂为极线。根据几何关系还可以确定投影点p₂在不同位置时，空间点P所对应的深度。

在图3中，由于基线是固定的，所以点O₁，O₂，p₁，p₂可以形成的稳定的三角形，从而基于关键点和投影点所确定的P点深度是较为精确的。

在一实施方式中，可以采用NCC(Normalized cross correlation归一化的交叉相关性)匹配方法，在右图的上述范围中寻找与p₁点相似的图像块，从而进一步缩小投影点p₂可能出现的范围。具体的，可以参照以下公式(5)确定两个图像块是否相似。

其中S(A,B)_NCC是两个图像块之间的相关性，A(i,j)是p₁点周围的具有一定大小的图像块的光度，B(i,j)是极线e₂p₂上相同大小的图像块的光度。

其中，接近1这表示两个图像块相似，接近0则不相似。

在一实施方式中，利用深度滤波器技术，将相似块中每一点所对应的P点的深度值从图像金字塔顶层向底层传播，获得深度值的概率分布形势。

步骤S230：基于所述投影点的获取范围，构建所述双目图像序列中两图像之间的静态约束。

具体的，静态约束中光度误差的能量函数可以表示为如上述公式(1)所示，光度误差的雅克比矩阵可以表示为，如公式(2)所示。

步骤S240：计算静态约束，确定深度值。

由于双目图像序列中两目摄像头的距离是固定已知的，因此能量函数中T^RL固定的。基于所确定投影点可能出现的范围，通过改变投影点的位置，使用高斯牛顿法以由粗到精的方式迭代计算光度误差的能量函数与雅克比矩阵直至收敛，从而获得光度误差的最小值，并确定最小光度误差所对应的深度值。

在一实施方式中，可以对所确定的深度值进行光束法平差(BA，BundleAdjustment)优化。其中，BA优化是指通过将设备的姿态和测量点的三维坐标作为未知参数，将影像上探测到的用于前方交会的特征点坐标作为观测数据从而进行平差得到最优的设备参数和世界坐标。

在一实施方式中，可以将所获得的深度值在图像金字塔中由下向上传播，优化图像金字塔中顶层图像中像素点的深度值。

参阅图4，图4是根据本申请另一实施方式的SLAM方法的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图4所示的流程顺序为限。如图4所示，本实施例包括：

步骤S410：获取一帧由可移动设备拍摄的双目图像序列，将其作为当前帧。

在一实施方式中，双目图像序列是由双目摄像设备拍摄的。在一实施方式中，双目图像序列可以是从双目摄像装置中直接获取的，也可以是从其他储存装置中调取的。仅仅为了方便描述，下文中将这两张图像分别称为左图和右图。

步骤S420：判断是否初始化。

在一实施方式中，在获取一帧双目图像序列后，还会判断是否已进行初始化。若为是，则进行步骤S440；若为否，则进行步骤S430。

步骤S430：进行初始化步骤。

在一实施方式中，在初始化过程中将获取的当前帧直接保存为关键帧，并且对当前帧的左、右图计算静态约束，确定当前帧图像上点的深度值。具体细节详见图2及其相关描述。

由于双目图像序列包括两张不同视角的图，可以通过计算这两张图确定图像上点的深度。初始化过程中无需随机设定深度，也无需通过设备运动后再进行初始化，从而实现快速初始化。

步骤S440：确定可移动设备的位姿。

在一实施方式中，可以将最近保存的关键帧确定为参考关键帧。设定参考关键帧中关键点与其在当前帧中投影点的深度相同，迭代计算当前帧与参考关键帧之间的动态约束，获得两帧图像之间的动态光度误差最小值。动态光度误差最小时所对应的位姿，即为当前帧的位姿。

步骤S450：判断当前帧是否为关键帧。

在一实施方式中，可以通过计算当前帧双目图像序列中两张图像之间的静态约束，确定当前帧的静态光度误差。

在一实施方式中，在所述当前帧满足预定条件时，将当前帧保存为关键帧。其中，预定条件为所述当前帧中融合光度误差小于阈值的点足够多。例如，可以通过判断当前帧中融合光度误差小于阈值的点是否超过40％。再例如，在当前帧中融合光度误差小于阈值的点没有超过40％时，可以扩大阈值至原阈值的1.5倍，并对当前帧的融合光度误差多进行一次的迭代计算；再判断当前帧中小于扩大阈值的点是否超过40％。

融合光度误差可以是由动态光度误差和静态光度误差融合而成。具体的，通过公式(6)将动态光度误差与静态光度误差融合，计算融合光度误差。

其中，

表示融合光度误差，

表示静态光度误差，

表示动态光度误差，λ为权重。动态光度误差为当前帧与参考关键帧之间的光度误差，静态光度误差为所述当前帧的双目图像序列中图像之间的光度误差。

在一实施方式中，判断当前帧是否为关键帧，若为是，则进行步骤S470，若为否，则进行步骤S460。

步骤S460：删除当前帧。

若当前帧不是关键帧，则表示该帧跟踪失败。在一实施方式中，可以将通过计算静态误差获得的当前帧中点的深度更新至参考关键帧中，并删除当前帧。通过更新参考关键帧可以在有很多帧跟踪失败的情况下仍能保证定位的实时性。

步骤S470：构建地图。

在一实施方式中，设定滑动窗口，滑动窗口中包括若干关键帧。在一实施方式中，对滑动窗口中的关键帧进行联合优化位姿。在对滑动窗口中的关键帧进行联合优化位姿的过程中，根据关键点在所有关键帧中的逆深度(即深度的逆值，可以理解为深度值的倒数)范围得到极线搜索范围，并通过高斯牛顿法不断的迭代优化，在极线上按照一定的步长进行搜索与当前关键帧中该关键点光度误差最小的位置，直至该关键点在当前关键帧与其他关键帧中的位姿增量足够小。然后调整相机内参，亮度参数，使用类L-M的方法来优化关键点的逆深度值。其中类L-M的方法是指类似L-M方法，L-M(Levenberg-Marquardt)方法，是非线性回归中回归参数最小二乘估计的一种估计方法。

在一实施方式中，将所述当前帧中的点补充至已保存的所述地图中，获得更新地图。已保存的地图可以是指在获取当前帧之前已经建立的地图，也可以为经滑动窗口中的关键帧联合优化后的地图。

在一实施方式中，通过计算所述更新地图与所述当前帧之间的第二融合约束，判断所述补充的点是否需要删除，完成地图构建。其中所述第二融合约束是由所述当前帧的所述双目图像序列中两个图像之间的静态约束和所述当前帧与所述地图之间的动态约束组成的。通过计算第二融合约束，可以确定当前帧中点的静态光度误差和当前帧与地图之间点的动态光度误差，若两个光度误差值差距较大，则将该点从地图中删除。经过上述步骤的优化，完成地图构建。

在地图构建中，不仅参考了当前帧与地图之间的光度误差，也参考了当前帧两图之间的静态误差，从而使得地图中的点的位置更加准确，提高了所构建地图的准确度。

步骤S480：删除滑窗内的冗余关键帧。

在一实施方式中，滑动窗口中关键帧的数量需要保持在一定范围内。例如，滑动窗口中可以包括5-7个关键帧。在滑动窗口中关键帧的数量超过一定值时，可以将与最新关键帧距离最大的关键帧在滑窗内删除。

在SLAM方法中，当有新的关键帧加入时，还可以进行闭环检测步骤，进一步对地图进行优化。参阅图5，图5是根据本申请一实施方式中SLAM方法中的闭环检测方法的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图5所示的流程顺序为限。如图5所示，本实施例包括：

步骤510：确定进行闭环检测的点。

在一些实施方式中，对滑动窗口内的关键帧提取ORB特征，计算描述子，并构建数据库。例如，可以采用词袋方法构建数据库。通过查询数据库为当前关键帧提取出闭环候选点，然后将这些点与当前关键帧中的ORB特征点进行匹配，确定需要进行闭环检测的点。

在本申请中，提取ORB特征，计算描述子的步骤仅限于在进行闭环检测的过程中，不会影响前期运动跟踪过程的运算速度。并且，仅针对滑动窗口内的关键帧提取闭环候选点，而不是对所有关键帧进行提取，可以有效的减少运算量，提高运算速度。

步骤520：进行闭环检测。

在这一实施方式中，基于闭环检测点在当前关键帧中的深度值，通过ICP(Iterative Closest Point，迭代就近点)方法建立闭环检测点所在的前一关键帧，即闭环候选帧，上3D-3D的几何约束关系，如公式(7)所示。其中ICP方法是一种高层次的基于自由形态曲面的配准方法。在一实施方式中，执行RANSAC(Random Sample Consensus)和PnP(Perspective-n-Points，透视n点投影)算法，以计算后一关键帧，即当前关键帧，上3D-2D约束关系，如公式(8)所示。其中，RANSAC是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。其中，之后如公式(9)所示，融合两个约束，获得第三融合约束。通过高斯牛顿法不断迭代优化第三融合约束，计算出最优的可移动设备的位姿，并将该位姿更新至已保存的地图中，优化地图。

其中，

是闭环检测点与当前关键帧上关键点的深度误差，

是当前关键帧上关键点投影到闭环关键帧上与闭环检测点的重投影误差，ω₁、ω₂是用于平衡不同测量单位的权重，q是闭环候选帧中的闭环检测点，p是当前关键帧中的特征点，d是对应的逆深度，S_cr为闭环候选帧与当前关键帧的相对位姿。

请参阅图6，图6是根据本申请一实施方式中SLAM方法中的闭环检测方法的流程示意图。该实施方式中，包括获取模块610和计算模块620。需要说明的是，本实施方式的装置可以执行上述方法中的步骤，相关内容的详细说明请参见上述方法部分，在此不再赘叙。

在一实施方式中，获取模块610用于从双目摄像装置中直接获取双目图像序列，也可以是从其他储存装置中调取双目图像序列。在一实施方式中，获取模块610还可以用于对所获取的双目图像序列构建图像金字塔。

在一实施方式中，计算模块620用于计算当前帧与参考关键帧之间的第一融合约束，确定所述可移动设备的位姿；其中所述第一融合约束是由所述参考关键帧的所述双目图像序列中两个图像之间的静态约束和所述当前帧与所述参考关键帧之间的动态约束组成的。

在一实施方式中，计算模块620可以用于计算参考关键帧的双目图像序列中的两张图像之间的静态约束直至收敛，确定所述参考关键帧中至少部分点的深度。

在一实施方式中，计算模块620可以用于基于所确定的参考关键帧中至少部分点的深度，计算当前帧与参考关键帧之间的动态约束直至收敛，从而确定所述可移动设备的位姿。

在一实施方式中，计算模块620可以用于确定图像中的关键点；基于所述两图像之间的几何关系，确定关键点在另一个图像中的投影点的获取范围；基于所述投影点的获取范围，构建所述双目图像序列中两图像之间的静态约束。

在一实施方式中，计算模块620还可以用于判断是否初始化，并进行初始化步骤，判断当前帧是否为关键帧以及构建地图。

在一实施方式中，SLAM装置还包括闭环检测模块，通过对新保存的关键帧进行闭环检测，优化地图。

请参阅图7，图7是根据本申请一实施方式中计算机设备的结构示意图。该实施方式中，计算机设备700包括相互耦接的处理器710和双目拍摄装置720。

处理器710还可以称为CPU(Central Processing Unit，中央处理单元)。处理器710可能是一种集成电路芯片，具有信号的处理能力。处理器710还可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

双目拍摄装置720可能是具有两个并排安装的光学系统并对齐指向相同的方向的拍摄装置，用于拍摄双目图像，形成双目图像序列。

计算机设备700可以进一步包括存储器(图中未示出)，用于存储处理器710运行所需的指令和数据。

处理器710用于执行指令以实现上述本申请SLAM方法任一实施例及任意不冲突的组合所提供的方法。

请参阅图8，图8是根据本申请一实施方式中具有存储功能的装置的结构示意图。本申请实施例的具有存储功能的装置800存储有指令，该指令被执行时实现本申请SLAM方法任一实施例以及任意不冲突的组合所提供的方法。其中，该指令可以形成程序文件以软件产品的形式存储在上述具有存储功能的装置中，以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式方法的全部或部分步骤。而前述的具有存储功能的装置包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质，或者是计算机、服务器、手机、平板等终端设备。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

以上所述仅为本发明的实施方式，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (11)

1.一种即时定位与地图构建方法，其特征在于，所述方法包括：

获取一帧由可移动设备拍摄的双目图像序列，将其作为当前帧；

计算所述当前帧与参考关键帧之间的第一融合约束，确定所述可移动设备的位姿；其中所述第一融合约束是由所述参考关键帧的所述双目图像序列中两图像之间的静态约束和所述当前帧与所述参考关键帧之间的动态约束组成的；

在所述当前帧满足预定条件时，将所述当前帧保存为关键帧，并且进行地图构建；

其中，所述预定条件为所述当前帧中融合光度误差小于第二阈值的点足够多；所述融合光度误差由动态光度误差和静态光度误差融合而成；所述动态光度误差为当前帧与参考关键帧之间的所述光度误差，且通过计算当前帧与参考关键帧之间的动态约束确定；所述静态光度误差为所述当前帧的所述双目图像序列中图像之间的所述光度误差，且通过计算所述当前帧的所述双目图像序列中图像之间的静态约束确定。

2.根据权利要求1所述的方法，其特征在于，所述静态/动态约束包括所述两图像中至少部分点与投影点之间的光度误差的能量函数以及所述光度误差的雅克比矩阵。

3.根据权利要求2所述的方法，其特征在于，所述通过计算所述当前帧与参考关键帧之间的第一融合约束，确定所述可移动设备的位姿包括：

计算所述参考关键帧的所述两图像之间的静态约束直至收敛，确定所述参考关键帧中所述至少部分点的深度；

基于所述至少部分点的深度，计算所述当前帧与所述参考关键帧之间的动态约束直至收敛，从而确定所述可移动设备的位姿。

4.根据权利要求2或3所述的方法，其特征在于，所述能量函数至少由两图像之间的位姿，深度，以及相机内参系数确定；

所述光度误差的所述雅克比矩阵至少与所述关键点和/或投影点的光度、位姿以及深度相关。

5.根据权利要求1所述的方法，其特征在于，所述静态约束的构建方法包括：

确定所述两图像上的关键点；

基于所述两图像之间的几何关系，确定所述关键点在另一个图像中的投影点的获取范围；

基于所述投影点的获取范围，构建所述双目图像序列中两图像之间的静态约束。

6.根据权利要求4所述的方法，其特征在于，所述关键点的确定方法，包括：

将所述图像进行网格划分，确定每一格中需获取关键点的数量；

对网格内的图像进行搜索，获取像素大于第一阈值的点，将其作为所述关键点；

其中，初始搜索步长为预设值，判断利用所述初始搜索步长是否能够获得足够数量的关键点，

若为是，则停止搜索；

若为否，则扩大搜索步长继续搜索。

7.根据权利要求1所述的方法，其特征在于，所述计算所述当前帧与参考关键帧之间的第一融合约束之前包括：

通过计算所述当前帧两图像之间的静态约束，确定所述当前帧中至少部分点的深度；并且将所述当前帧保存为所述关键帧，以完成初始化。

8.根据权利要求1所述的方法，其特征在于，所述在所述当前帧满足预定条件时，将所述当前帧保存为关键帧，并且进行地图构建包括：

将所述当前帧中的点补充至已保存的所述地图中，获得更新地图；

通过计算所述更新地图与所述当前帧之间的第二融合约束，判断所述补充的点是否需要删除，完成地图构建；其中所述第二融合约束是由所述当前帧的所述双目图像序列中两个图像之间的静态约束和所述当前帧与所述地图之间的动态约束组成的。

9.根据权利要求1所述的方法，其特征在于，所述方法包括：

设定滑动窗口，所述滑动窗口包括若干关键帧；

基于滑动窗口内已保存的所述若干关键帧确定进行闭环检测的候选点；

对所述候选点进行闭环检测。

10.一种计算机设备，其特征在于，包括相互耦接的处理器和双目拍摄装置，所述双目拍摄装置用于拍摄双目图像，形成所述双目图像序列，所述处理器用于执行指令以实现如权利要求1-9任一项所述的即时定位与地图构建方法。

11.一种具有存储功能的装置，其特征在于，所述装置存储有程序，所述程序被执行时能够实现如权利要求1-9任一项所述的即时定位与地图构建方法。

Images