CN111322993A - 一种视觉定位方法和装置 - Google Patents

Abstract

本申请公开了一种视觉定位方法，应用于直接法里程计，该方法包括，将来自图像数据流的每帧图像输入直接法里程计进行前端处理，并选取出关键帧；依据里程计的运动状态触发基于关键帧的视觉重定位；将视觉重定位结果的约束融合到直接法里程计目标函数中；通过最小化目标函数求解相机位姿。本申请结合了直接法里程计精度高和适用于少纹理区域的优点以及特征点法易于构建地图进行重定位消除累积误差的优点，有效地降低了直接法里程计的累积误差。

Description

一种视觉定位方法和装置

技术领域

本发明涉及计算机视觉领域，特别地，涉及计算机视觉中的一种视觉定位方法以及装置。

背景技术

视觉定位是通过视觉信息和构建的视觉地图来获取稳定准确的相机位姿，例如，机器人通过图像信息和特征地图信息的匹配获取机器人当前相机位姿的过程。相机位姿用六个维度描述，分别是位置(三维坐标位置)、姿态(三维)。

视觉里程计(VO，visual odometry)是进行视觉定位常用方法之一，目标是根据拍摄的图像估计相机的运动，具体而言，是通过图像信息增量式地获取相机位姿的方法，其通过分析一系列图像序列来确定机器人(相机)的朝向和姿态，即，估计机器人的位置和姿态随时间的变化。视觉里程计基本过程是：获取视频流(主要为灰度图像)，记录在t和t+1时刻获得的图像为I_{t}和I_{t+1}，通过相机标定获得相机的内部参数，将获得图像和摄像头的内部参数作为视觉里程计的输入，通过算法模型处理后，输出每一帧图像所对应的相机位置和姿态。

按照视觉里程计中的算法模型的处理方式，视觉里程计包括直接法和特征点法。其中，直接法里程计方法使用像素点，通过最小化某些像素点的光度误差来计算相机位姿与图像点的位置，所谓光度误差即是最小化目标函数，通常由图像之间的误差决定。

为方便说明直接法的原理，以下以DSO(Direct Sparse Odometry)算法为例来说明。直接法将数据关联(data association)与位姿估计(pose estimation)放在了一个统一的非线性优化问题中，由于这个原因，成为一直求解一个比较复杂的优化问题。每一个三维点，从某个主导帧(host frame)出发，乘上深度值之后投影至另一个目标帧(targetframe)，从而建立一个投影残差(residual)。只要残差在合理范围内，就可以认为这些点是由同一个点投影的。从数据关联角度看，在这个过程中并没有a1-b1,a2-b2这样的关系，也可能存在a1-b1,a2-b1,a3-b1这样的情况，其目标是尝试把每个点投影到所有帧中，计算它在各帧中的残差，而并不在意点和点之间的一一对应关系。

从后端来看，直接法视觉里程计使用一个由若干个关键帧组成的滑动窗口作为它的后端。这个窗口在整个VO过程中一直存在，并有一套方法来管理新数据的加入以及老数据的去除。具体来说，这个窗口通常保持5到7个关键帧。前端追踪部分，会通过一定的条件，来判断新来的帧是否可作为新的关键帧***后端。同时，如果后端发现关键帧数已经大于窗口大小，也会通过特定的方法，选择其中一个帧进行去除。被去除的帧并不一定是时间线上最旧的那个帧，而是会有一些复杂条件的。

后端除了维护这个窗口中的关键帧与图像点外，还会维护与优化相关的结构。例如，试图将每个先前关键帧中的图像点投影到新关键帧中，形成残差项。同时，会在新关键帧中提取未成熟点，并希望它们演变成正常图像点。在实际当中，由于运动、遮挡的原因，部分残差项会被当作外点(outlier)，最终剔除；也有部分未成熟图像点无法演化成正常图像点，最终被剔除。所有的残差加起来，就构成了需要求解的优化问题，优化的目标是最小化光度误差。

这样，滑动窗口内部构成了一个非线性最小二乘问题。表示成因子图(或图优化)的形式，如附图1所示。每一个关键帧的状态为八维：六自由度的运动位姿加上两个描述光度的参数；每个图像点的状态变量为一维，即该点在主导帧(Host)里的逆深度。于是，每个残差项(或能量项E)，将关联两个关键帧与一个逆深度。事实上，还有一个全局的相机内参数亦参与了优化，但未在此图中表示。

参见图2所示，图2为直接法视觉里程计的VO流程。每当图像数据流到来时，将按照图示的流程处理该图像的信息。从图2可以简单总结出VO的过程：

对于非关键帧，仅计算它的位姿，并用该帧图像更新每个未成熟点的深度估计；

后端仅处理关键帧部分的优化。除去一些内存维护操作，对每个关键帧主要做的处理有：增加新的残差项、去除错误的残差项、提取新未成熟点。

从上述直接法视觉里程计方法的处理和原理可以看出，该方法能够很好地应对纹理较少的场合；同时因为不需要提取特征和特征匹配，可以得到很高的帧率；并且因为利用了所有的图像信息，得到的相机位姿估计会比特征点法更准确。但是现有的直接法里程方法进行跟踪时，由于没有闭环检测，累计误差会越来越大。

发明内容

本发明提供了一种视觉定位方法，以减少直接法里程计的累计误差。

本发明提供的一种视觉定位方法是这样的：

一种视觉定位方法，应用于直接法里程计，该方法包括，

将来自相机的图像数据流的每帧图像输入直接法里程计进行前端处理，并选取出关键帧；

依据所述直接法里程计的运动状态，触发基于关键帧的视觉重定位；

将视觉重定位结果作为约束，融合到直接法里程计的目标函数中，得到融合后的目标函数；

通过最小化所述融合后的目标函数，求解相机位姿。

其中，所述依据所述直接法里程计的运动状态触发基于关键帧的视觉重定位包括，累计当前关键帧的数量是否达到第一阈值，如果是，则触发所述视觉重定位，并将累计的关键帧数量清零，否则，禁止触发。

所述将视觉重定位结果作为约束和融合到直接法里程计的目标函数中包括，将所述视觉重定位结果作为约束，添加至直接法计里程后端优化的约束中。

所述将视觉重定位结果作为约束和融合到直接法里程计的目标函数中包括，在构建的融合重定位结果约束的目标函数中，重定位结果约束被加权，并且加权的权重根据视觉重定位结果的置信度而动态调整。

所述权重为视觉重定位结果进行置信度估计的结果，所述置信度估计的结果由角速度、线速度、视觉重定位时特征点的数量、视觉重定位结果与直接法里程计定位结果的差值决定。

所述将视觉重定位结果作为约束融合到直接法里程计的目标函数包括，

将相邻视觉重定位之间的所有关键帧进行局部图优化所得到相机位姿作为约束，添加至直接法里程计的后端优化的窗口中；其中，所述约束为：视觉重定位结果经过局部图优化之后得到的关键帧帧间相机位姿与直接法里程计得到的该关键帧帧间相机位姿在没有误差的情况下相等；

对于任一图像点，按照如下式子构造融合的误差函数E_new：

E_new＝E_photo+wE_{relocalization}

本申请另一方面提供了一种视觉定位装置，该装置包括具有直接法里程计功能的处理器，其中，所述处理器

将来自相机的图像数据流的每帧图像输入进行直接法里程计进行前端处理，并选取出关键帧；

依据所述直接法里程计的运动状态，触发基于关键帧的视觉重定位；

将视觉重定位结果作为约束，融合到所述直接法里程计的目标函数中，得到融合后的目标函数；

通过最小化所述融合后的目标函数，求解相机位姿。

本发明实施例在直接法里程计过程中，通过在需要的时机下触发视觉重定位、并将视觉重定位结果的约束融合直接法里程计的光度误差来构造目标函数，使得直接法里程计的累积误差得到消除；其中视觉重定位采用了特征点法重定位，结合了直接法里程计精度高和适用于少纹理区域的优点以及特征点法易于构建地图进行重定位消除累积误差的优点。

附图说明

图1为现有直接法里程计后端滑动窗口内部构成的非线性最小二乘问题的图优化示意图。

图2为现有直接法视觉里程计的VO流程。

图3为本实施例视觉定位方法的一种总体示意图。

图4为本发明实施例直接法里程方法与重定位融合的一种流程示意图。

图5为本发明实施例融合重定位结果的优化因子图。

图6为本发明实施例将重定位结果与直接法里程计融合时滑动窗口内部所构成的非线性最小二乘问题的因子图(或图优化)的形式。

图7为本发明实施例离线建图过程和基于特征点法实现视觉重定位的一种流程示意图。

图8为本发明实施例的视觉定位装置的一种示意图。

具体实施方式

首先，对本申请中的部分用语进行解释说明。

关键帧：相机满足一定运动条件或者场景变化的图像帧，选择关键帧进行相关计算用于减少整体的计算量，不需要对每一帧进行复杂的计算。

成熟点：深度的置信度较高、或者深度方差较小的点。

未成熟点：深度的置信度较低、或者深度方差较大的点。

为了使本申请的目的、技术手段和优点更加清楚明白，以下结合附图对本申请做进一步详细说明。

本发明实施例将直接法里程方法与视觉重定位进行融合，在采用直接法里程方法进行视觉定位的过程中，根据里程计的运动状态触发视觉重定位，例如，当累计的关键帧数量达到第一阈值时触发视觉重定位，并融合视觉重定位的结果来消除直接法里程计视觉定位的累积误差。

参见图3所示，图3为本实施例视觉定位方法的一种总体示意图。整体分为两个部分。第一部分是特征地图离线构建，通过对环境进行遍历，利用特征点法构建特征地图，以用于重定位时加载；第二部分是视觉里程计部分，主体为直接法里程计，包括了直接法里程计前端和直接法里程计后端。

在直接法里程计运行过程中，累积误差会逐渐增大，通过定期地进行重定位，融合重定位结果来消除累积误差。来自相机的图像数据流每帧图像帧输入至直接法里程计，直接法里程计前端对输入的图像帧进行处理，直接法里程计进行前端跟踪，根据里程计的运动状态触发视觉重定位，例如，对前端跟踪过程中选取的关键帧数量进行累计，当累计的关键帧数量达到第一阈值时触发视觉重定位，视觉重定位通过对当前帧的图像特征点和离线构建的特征地图进行匹配，对匹配的特征点进行求解得到当前帧的重定位结果；对重定位结果进行置信度计算，将计算得到的置信度作为融合重定位结果到直接法里程计的权重；基于相邻重定位之间的所有关键帧进行局部图优化，将局部图优化结果按照所述权重进行加权后作为约束添加至直接法里程计的后端优化窗口中，直接法里程计后端基于当前重定位所融合结果进行后端优化。参见图4所示，图4为以DSO算法为例来说明本发明实施例直接法里程方法与重定位融合的一种流程示意图。在直接法里程计方法中，通常，将获得相机位姿初值的过程称为直接法前端，基于相机位姿初值进行迭代优化的过程称为直接法后端。结合图4而言，即，前端跟踪、关键帧获取步骤为直接法前端，将点帧管理、后端优化、以及触发视觉重定位等步骤称为直接法后端。

步骤401，对获取的图像进行预处理，包括图像去畸变、光度矫正等。若是双目视觉***，还包括双目矫正。

步骤402，判断初始化是否成功，如果没有成功，则进行初始化，通过初始化以得到相机的初始位姿。具体为，估计图像第一帧与第二帧之间的运动，通过单目或双目的初始化方式，确定相机的初始位姿。当初始化成功时，则执行步骤403。

步骤403，经过初始化之后，采用直接法进行帧间跟踪，在实际的定位过程中，通过提取像素灰度的梯度点、以及最小化参考帧和当前帧的光度误差来进行帧间相机运动估计，以求解相机姿态，得到相机位姿的初值。

步骤404，基于步骤403直接法进行帧间跟踪过程中的图像帧，选取出关键帧，如果无法选取出关键帧，也就是说，当选取的帧因不满足一定条件而作为非关键帧时，非关键帧不进入后端进行处理，这时，则执行步骤405，更新未成熟点的深度和/或方差，使得未成熟点深度更准确，方差更小，然后返回步骤401，否则，执行步骤406；步骤406，由于里程计运行过程中，累积误差不断增大，通过定期地进行视觉重定位清除累积误差，考虑到里程计运行的实时性，视觉重定位并非实时进行，而是定期进行的，比如，固定每5个关键帧或每10个关键帧触发一次重定位，由于里程计运行过程中，会通过一定条件，来判断新来的帧是否可作为新的关键帧***后端，使得通过关键帧跟踪了里程计的运动状态，从而从一个方面达到了根据里程计的运动状态来触发视觉重定位的效果，而对于重定位的频率可不作限定。

在具体实现上，例如，累计当前关键帧数量是否达到第一阈值，当达到第一阈值时，说明此时需要进行视觉重定位，则触发视觉重定位处理，执行步骤407的视觉重定位，在触发视觉重定位的同时，将当前关键帧数量的累计值清零，以便于为下一次视觉重定位的触发进行关键帧的数量统计；当视觉重定位处理结束后，执行步骤409；

当累计的关键帧数量未达到第一阈值时，则禁止触发视觉重定位，执行步骤408，更新未成熟点，标记需要删除的帧；将新关键帧加入后端优化；将旧的图像点投影至新关键帧，产生残差项；激活未成熟点，将未成熟点转为图像点；该处理的过程可视为点帧管理过程；点帧管理过程结束后，执行步骤410。

步骤409，相对于无重定位的后端图优化处理而言，由于相邻重定位之间的所有关键帧是无重定位后端图优化的局部，故这时所进行的图优化为局部图优化；相邻重定位之间的所有关键帧包括直接里程计前端选取出来的关键帧，其中有些关键帧可能仍在优化窗口中，有些关键帧已经边缘化。

基于相邻重定位(例如，相邻两次重定位)之间后端优化窗口中的所有关键帧进行局部图优化，通过局部图优化来确定相邻重定位之间所有关键帧对应的相机位姿，即局部图优化之后得到的相机位姿，并将该相机位姿作为约束添加到后端的优化窗口中，具体为：

其中，视觉重定位结果E_{relocalization}为视觉重定位过程中后端优化窗口中所有关键帧的局部图优化结果与直接法里程计定位结果构成的残差之和，i、j为关键帧号，n为相邻重定位之间的所有关键帧总数，T_r,ij是重定位结果经过局部图优化之后得到的关键帧帧间相机位姿，T_d,ij是直接法里程计得到的关键帧帧间相机位姿，这两个帧间相机位姿在没有误差的情况下应该相等，这就是重定位结果添加的约束。

上述局部图优化的过程可以嵌入至直接法里程计的后端优化之前，即在后端优化执行之前执行；也可以独立于后端优化，使得局部图优化在直接法里程计流程之外执行，在这种实施方式下，需要在得到重定位结果后，将重定位结果作为约束添加到直接法里程计的约束中。

步骤410，执行后端优化：当无重定位结果时，目标函数即为光度误差，最小化光度误差；当有重定位结果时，目标函数即为光度误差和视觉重定位结果的融合误差，最小化融合误差。

将视觉重定位结果的约束融合到直接法里程计中构造优化目标函数。在本发明实施例融合并不是简单的加权平均，而是一个紧耦合过程，即，将重定位结果作为约束添加到直接法里程计的约束中，其优化因子图如图5所示，图5为融合重定位结果的优化因子图。

对于任意图像点，光度误差E_photo为直接法里程计后端优化窗口中所有关键帧中该图像点的残差之和，视觉重定位结果

为视觉重定位过程中后端优化窗口中所有关键帧的局部图优化结果与直接法里程计定位结果构成的残差之和。

这样，融合重定位结果之前，直接法里程计原本的优化问题是最小化光度误差E_photo，在融合过程增加了重定位的结果约束E_{relocalization}后，融合后的最终的优化问题是最小化误差E_new，即，按照下式构造目标函数E_new，并通过最小化目标函数来求解相机位姿，由此，优化对象为相邻两次重定位之间的所有关键帧的相机位姿。

E_new＝E_photo+wE_{relocalization}

其中，w为重定位结果的置信度，用作后端窗口优化的权重；在无重定位触发时，所构造的目标函数中则无重定位结果的约束。

参见图6所示，图6示出了将重定位结果与直接法里程计融合时滑动窗口内部所构成的非线性最小二乘问题的因子图(或图优化)的形式。其中，图中光度残差E_p(E_photo)是直接法里程计原本的优化对象，重定位光度残差E_r(E_{relocalization})是进行了重定位的关键帧与特征地图重定位结果之间的残差，可以消除***的累积误差。整体的状态仍然是每个关键帧的八维状态，即六自由度的相机位姿和两个光度参数。

步骤411，后端优化处理后，后端进行移除外点、提取新关键帧为成熟点等的处理。

通过上述步骤的处理，将重定位结果作为约束直接添加到直接法里程计原本的优化问题中，并且，通过紧耦合的方式，将重定位结果的误差项添加到直接法里程计的优化框架下，使得算法的结果更优，更加鲁棒，能提高整体的鲁棒性和全局一致性。

参见图7所示，图7为本发明实施例离线建图过程和基于特征点法实现视觉重定位的一种流程示意图。

步骤701，当视觉重定位被触发时，对图像数据流的当前帧进行图像预处理，并进行特征点提取，将所提取的特点作为第一特征点。所述图像预处理包括，图像去畸变、高斯模糊、构建高斯金字塔、特征点和描述子提取等，若是双目视觉***，还包括双目矫正。

步骤702，将所述第一特征点与加载的特征地图进行匹配，通过将所述第一特征点与特征地图中特征点的匹配，构建当前帧图像与特征地图之间的数据关联，即，将在特征地图中的特征点匹配到当前帧图像的特征点。

步骤703，根据特征地图中所匹配的特征点，求解重定位结果。即，通过匹配的特征点，进行PnP求解，得到当前帧的重定位结果，也就是相机的位姿。

步骤704，对重定位结果进行置信度计算。由于环境中的干扰因素非常多，当前场景并不一定适合重定位，比如白色墙壁、雷同的长廊等，这使得重定位结果并不是那么可靠。而错误的重定位结果融合到里程计中反而会将里程计带偏。所以需要对重定位结果进行置信度计算，确保融合结果的可靠性。依据重定位的帧间运动估计与直接法里程计帧间运动估计之间的差值、特征点数量、角速度和线速度等因素估计当前帧的重定位结果的可靠程度，并基于此作为后端窗口优化的权重。重定位结果的置信度体现在重定位约束项权重w，与重定位状态以及里程计的运动状态相关，在角速度和线速度较大的情况下，权重w会比较小，并且重定位结果与里程计结果的差值ΔT和Δθ也作为权重的因子，重定位结果与里程计结果越接近，则权重w越大。此外，整体权重w还与重定位时的特征点数量相关，特征点数量n越大，则认为定位结果越可靠。基于此，重定位约束项权重w可以为：

w＝f(ω,ν,ΔT,Δθ,n)

具体实施方式之一可以为：

w＝αexp(β₅n-(β₁ω+β₂v+β₃||ΔT||+β₄||Δθ||))

其中，ω为角速度，ν为线速度，n为视觉重定位时的特征点数量，Δθ为重定位结果与里程计前端结果的角速度差值，ΔΤ为重定位结果与里程计前端结果的位移差值，β₁、β₂、β₃、β₄、β₅以及α为对应项的系数。

从上述的视觉重定位过程而言，本实施例的视觉重定位采用的是一种特征点法视觉重定位，该视觉重定位过程不同于视觉里程计定位过程，在视觉里程计定位过程中会因为里程计的积累而使得里程计的定位误差逐渐增大，而视觉重定位则不会有积累误差，因为重定位过程与历史帧对应的相机位姿信息没有关系，而只通过当前图像信息与特征地图的匹配来获得相机位姿；尽管各种干扰因素的存在使得重定位结果在某些场景下也会有比较大的误差，本实施例及时基于全局一致的地图，且通过里程计的运动状态以及重定位的系列参数来估计重定位结果的置信度，并基于该置信度进行重定位结果与直接法里程计定位结果融合的操作，减小了误差大的重定位结果对里程计的影响；这样，特征点法重定位结果与直接法里程计结果进行紧耦合后，使得定位算法整体能够综合特征点法重定位消除累积误差和直接法里程计可用于缺少纹理和特征的区域的优点，实现大环境下的全局一致的定位结果。

在上述步骤702中，所加载的特征地图以离线方式预先构建，通过对环境进行遍历，利用特征点法构建环境地图，比如ORB-SLAM，即时视觉与地图构建(SLAM，simultaneouslocalization and mapping)方法进行地图构建，通过闭环检测来消除累积误差，离线构建全局一致的特征地图。离线建图的目的在于在算法参数设置上不用考虑建图的实时性，而只需要考虑地图的精度。

参见图7所示，图7中虚线的上部分为本实施例生成特征地图的一种流程示意图。

步骤801，采集环境数据，用于离线构建环境地图。以机器人的工作姿态遍历机器人工作状态下会访问的环境，将场景中的视觉特征构建到特征地图中。所使用的视觉特征不限于ORB(Oriented Brief)、SIFT(Scale-Invariant Feature Transform)、SURF(SpeedUp Robust Features)和BRIEF(Binary Robust Independent Elementary Features)等。

步骤802，图像预处理。对图像进行必要的预处理，包括图像去畸变、高斯模糊、构建高斯金字塔、提取特征点和描述子等。若是双目视觉***，还包括双目矫正。

步骤803，帧间跟踪，即估计帧与帧之间的相对运动，以得到相机位姿。通过特征点和描述子进行特征点匹配，构建帧与帧之间的数据关联，并通过估计的帧间运动和图像状态选取关键帧，以便于基于关键帧进行后续的相机位姿优化操作。该过程为特征点法里程计的典型过程。

步骤804，对当前帧之前的一定范围内的关键帧对应相机位姿进行优化。具体为，通过关键帧与关键帧之间观测到的一些共同特征点，对该范围内的所有共同特征点和各帧构成重投影误差约束，并最小化特征点重投影误差，通过最小化特征点重投影误差来实现所述相机位姿的优化。

步骤805，闭环检测和优化。由于相机位姿累积误差非常大，还需要通过闭环检测提高地图的全局一致性，使得整个特征地图的精度得到提高，消除累积误差。在闭环检测中，利用图像特征点和描述子的词袋模型进行相似性评价，以检测图像场景的相似性，根据检测到具有相似图像场景的帧(为方便描述称为闭环帧)，构建闭环帧与闭环帧之间的数据关联，并将当前帧和闭环帧之间的相机位姿关系构成约束，该约束与历史帧信息无关，也就是不受累积误差影响。

通过上述流程，得到了特征地图。

离线地图构建目的在于为视觉重定位提供参考数据，而离线地图构建相对于在线的地图构建具有得优势是，相关参数设置可以尽可能地考虑地图精度而不用在意建图的实时性。而构建特征地图的原因在于直接法里程计无法构建用于重定位的地图，重定位过程需要鲁棒性较好的数据关联，只有通过鲁棒特征才能实现，所使用的特征不局限于ORB、SIFT、SURF、BRIEF等。

参见图8所示，图8为本发明实施例的视觉定位装置的一种示意图。该装置包括具有直接法里程计功能的处理器，其中，所述处理器：

将来自图像数据流的每帧图像输入直接法里程计进行前端处理，并选取出关键帧；当需要重定位时触发基于关键帧的视觉重定位；

将视觉重定位结果融合直接法里程计构造目标函数；通过最小化目标函数求解相机位姿。

该装置还包括有存储器，用于存储离线构建的特征地图，以供进行视觉重定位时进行地图匹配。

上述装置可应用于携带相机/摄像头的设备，如移动机器人，无人机，智能终端等。

本发明实施例中的直接法里程计不限于LSD(Large-Scale Direct MonocularSLAM)、DSO(Direct Sparse Odometry)，也不限于单目、双目或者多目视觉***。

应认识到，本文在下面描述的示例可包括各种部件和特征。在不背离装置、方法以及非瞬时性计算机可读介质的范围的情况下，可去除和/或修改这些部件和特征中的一些。还应认识到，在下面的描述中，给出特定细节以提供对示例的透彻理解。然而，应理解，示例可在没有限制这些具体细节的情况下被实践。在其它情况下，可以不详细描述已知的方法和结构，以避免不必要的模糊对示例的描述。另外，示例可以彼此结合使用。

在说明书中对“实施例”的参考或类似语言意味着结合示例描述的具体特征、结构或特性包括在至少一个实施例中，但不必一定在其它实施例中。在说明书中的各种位置的短语“在一个实施例中”或类似短语的各种情况不一定总参考相同的示例。如本文所使用的，部件是在硬件上执行的硬件和软件的结合以提供给定的功能。

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

Claims (23)

1.一种视觉定位方法，其特征在于，该方法包括，

将来自相机的图像数据流的每帧图像输入直接法里程计进行前端处理，并选取出关键帧；

依据所述直接法里程计的运动状态，触发基于关键帧的视觉重定位；

将视觉重定位结果作为约束融合到直接法里程计的目标函数中，得到融合后的目标函数；

通过最小化所述融合后的目标函数，求解相机位姿。

2.如权利要求1所述的视觉定位方法，其特征在于，所述依据所述直接法里程计的运动状态触发基于关键帧的视觉重定位包括，累计当前关键帧的数量是否达到第一阈值，如果是，则触发所述视觉重定位，并将累计的关键帧的数量清零，否则，禁止触发。

3.如权利要求1所述的视觉定位方法，其特征在于，所述将视觉重定位结果作为约束融合到直接法里程计的目标函数中包括，将所述视觉重定位结果作为约束，添加至直接法计里程后端优化的约束中。

4.如权利要求1所述的视觉定位方法，其特征在于，所述将视觉重定位结果作为约束融合到直接法里程计的目标函数中包括，在构建的融合重定位结果约束的目标函数中，重定位结果约束被加权，并且加权的权重根据视觉重定位结果的置信度而动态调整。

5.如权利要求4所述的视觉定位方法，其特征在于，所述权重为对视觉重定位结果进行置信度估计的结果，

所述置信度估计的结果由角速度、线速度、视觉重定位时特征点的数量、视觉重定位结果与直接法里程计定位结果的差值决定。

6.如权利要求5所述的视觉定位方法，其特征在于，所述置信度估计按照如下式子计算：

w＝αexp(β₅n-(β₁ω+β₂v+β₃||ΔT||+β₄||Δθ||))

其中，ω为角速度，ν为线速度，n为视觉重定位时的特征点数量，Δθ为重定位结果与里程计前端结果的角速度差值，ΔΤ为重定位结果与里程计前端结果的位移差值，β₁、β₂、β₃、β₄、β₅以及α为对应项的系数。

7.如权利要求4-6任一所述的视觉定位方法，其特征在于，所述将视觉重定位结果作为约束融合到直接法里程计的目标函数包括，

将相邻视觉重定位之间的所有关键帧进行局部图优化，得到相邻视觉重定位之间关键帧帧间相机位姿；

将得到的相邻视觉重定位之间关键帧帧间相机位姿作为约束，添加至直接法里程计的后端优化的窗口中；其中，所述约束为：视觉重定位结果经过局部图优化之后得到的关键帧帧间相机位姿与直接法里程计得到的该关键帧帧间相机位姿在没有误差的情况下相等；

对于任一图像点，按照如下式子构造融合的误差函数E_new为目标函数：

E_new＝E_photo+wE_{relocalization}

其中，E_photo为直接法里程计后端优化窗口内所有关键帧中该图像点的残差之和，E_{relocalization}为视觉重定位过程中后端优化窗口中所有关键帧的局部图优化结果与直接法里程计定位结果构成的残差之和，w为视觉重定位结果的置信度估计结果。

8.如权利要求7所述的视觉定位方法，其特征在于，所述相邻视觉重定位之间的所有关键帧为相邻两次视觉重定位之间的所有关键帧。

9.如权利要求1所述的视觉定位方法，其特征在于，所述视觉重定位为特征点法重定位；

所述特征点法重定位包括，

当视觉重定位被触发时，对图像数据流的当前帧进行图像预处理，并提取当前帧的特征点，得到第一特征点；

将所述第一特征点与离线构建的特征地图进行匹配，得到特征地图中与所述第一特征点相匹配的第二特征点，

根据所述第二特征点，求解视觉重定位结果。

10.如权利要求9所述的视觉定位方法，其特征在于，所述离线构建的特征地图的构建包括，

采集环境图像数据，对采集的图像数据进行预处理；

进行帧间跟踪，得到相机位姿；

对当前帧之前的一定范围内的关键帧，将关键帧与关键帧之间的所有共同特征点和各帧构成重投影误差约束，并最小化特征点重投影误差，得到优化的相机位姿；

检测图像场景的相似性，根据检测到具有相似图像场景的闭环帧，构建闭环帧与闭环帧之间的数据关联，并将当前帧和闭环帧之间的相机位姿关系构成约束，得到特征地图。

11.如权利要求10所述的视觉定位方法，其特征在于，所述预处理包括，图像去畸变、双目视觉***的双目矫正、高斯模糊、构建高斯金字塔、提取特征点和描述子。

12.一种视觉定位装置，其特征在于，该装置包括具有直接法里程计功能的处理器，其中，所述处理器

将来自相机的图像数据流的每帧图像进行直接法里程计前端处理，并选取出关键帧；

依据所述直接法里程计的运动状态触发，基于关键帧的视觉重定位；

将视觉重定位结果作为约束，融合到所述直接法里程计的目标函数中，得到融合后的目标函数；

通过最小化所述融合后的目标函数，求解相机位姿。

13.如权利要求12所述的装置，其特征在于，所述依据所述直接法里程计运动状态触发基于关键帧的视觉重定位包括，累计当前关键帧的数量是否达到第一阈值，如果是，则触发所述视觉重定位，并将累计的关键帧的数量清零，否则，禁止触发。

14.如权利要求12所述的装置，其特征在于，将视觉重定位结果作为约束，融合到所述直接法里程计的目标函数中包括，将视觉重定位结果作为约束，添加至直接法计里程后端优化的约束中。

15.如权利要求12所述的装置，其特征在于，将视觉重定位结果作为约束融合到所述直接法里程计的目标函数中包括，在构建的融合重定位结果约束的目标函数中，重定位结果约束被加权，并且加权的权重根据视觉重定位结果的置信度而动态调整。

16.如权利要求15所述的装置，其特征在于，所述权重对视觉重定位结果进行置信度估计的结果，所述置信度估计的结果由角速度、线速度、视觉重定位时特征点的数量、视觉重定位结果与直接法里程计定位结果的差值决定。

17.如权利要求16所述的装置，其特征在于，所述置信度估计按照如下式子计算：

w＝αexp(β₅n-(β₁ω+β₂v+β₃||ΔT||+β₄||Δθ||))

其中，ω为角速度，ν为线速度，n为视觉重定位时的特征点数量，Δθ为重定位结果与里程计前端结果的角速度差值，ΔΤ为重定位结果与里程计前端结果的位移差值，β₁、β₂、β₃、β₃、β₅、α为对应项的系数。

18.如权利要求15至17任一所述的装置，其特征在于，所述将视觉重定位结果作为约束融合到所述直接法里程计的目标函数中包括，

将相邻视觉重定位之间的所有关键帧进行局部图优化，得到相邻视觉重定位之间关键帧帧间相机位姿；

将所述相邻视觉重定位之间关键帧帧间相机位姿作为约束，添加至直接法里程计的后端优化的窗口中；

其中，所述约束为：视觉重定位结果经过局部图优化之后得到的关键帧帧间相机位姿与直接法里程计得到的该关键帧帧间相机位姿在没有误差的情况下相等；

对于任一图像点，按照如下式子构造融合的误差函数E_new：

E_new＝E_photo+wE_{relocalization}

其中，E_photo为直接法里程计后端优化窗口中所有关键帧中该图像点的残差之和，E_{relocalization}为视觉重定位过程中后端优化窗口中所有关键帧的局部图优化结果与直接法里程计定位结果构成的残差之和，w为视觉重定位结果的置信度估计结果。

19.如权利要求18所述的装置，其特征在于，所述相邻视觉重定位之间的所有关键帧为相邻两次视觉重定位之间的所有关键帧。

20.如权利要求12所述的装置，其特征在于，所述视觉重定位为特征点法重定位，

所述特征点法重定位包括，

当视觉重定位被触发时，对图像数据流的当前帧进行图像预处理，并提取当前帧的特征点，得到第一特征点；

将所述第一特征点与离线构建的特征地图进行匹配，得到特征地图中与所述第一特征点相匹配的第二特征点，

根据所述第二特征点，求解视觉重定位结果。

21.如权利要求20所述的装置，其特征在于，该装置还包括，存储有离线构建的特征地图的存储器，

所述特征地图的构建包括，

采集环境图像数据，对采集的图像数据进行预处理；

进行帧间跟踪，得到相机位姿；

对当前帧之前的一定范围内的关键帧，将关键帧与关键帧之间的所有共同特征点和各帧构成重投影误差的约束，并最小化特征点重投影误差，得到优化的相机位姿；

检测图像场景的相似性，根据检测到具有相似图像场景的闭环帧，构建闭环帧与闭环帧之间的数据关联，并将当前帧和闭环帧之间的相机位姿关系构成约束，得到特征地图。

22.如权利要求20或21所述的装置，其特征在于，所述预处理包括，图像去畸变、双目视觉***的双目矫正、高斯模糊、构建高斯金子塔、提取特征点和描述子。

23.一种非瞬时性计算机可读存储介质，包括直接法里程计指令，其特征在于，还包括在由设备的处理器执行时使所述处理器实现权利要求1至12任一所述视觉定位方法的指令。

Images