CN113744315B - 一种基于双目视觉的半直接视觉里程计 - Google Patents

Abstract

本发明公开了一种基于双目视觉的半直接视觉里程计，属于计算机视觉技术领域。首先，双目视觉像机识别待检测区域，得到左右图像，采用Sobel边缘检测与SAD匹配对左右图像进行粗探测，寻找左右图像的边缘最佳匹配点。然后，利用机载相机参数，提取完成粗探测的左右图像的视差直方图，基于视差直方图分配权重，分割出左右图像中的静态物体区域。最后，对划分出静态物体区域的左右图像，利用光流法检测图像环境，跟踪图像特征点，判断连续跟踪特征点成功的帧数是否少于20帧，若是，则通过直接法估计相机位姿；若否，则重新选择图像，使用特征点匹配法估计相机位姿。本发明在位姿估计的速度与精度上都获得较好的性能。

Description

一种基于双目视觉的半直接视觉里程计

技术领域

本发明属于计算机视觉技术领域，具体涉及一种基于双目视觉的半直接视觉里程计。

背景技术

双目视觉是使用两个固定相机来同时获得某一时刻的图像，然后通过相似三角形原理计算物体在不同相机的成像的位置差(即视差)来进行3D重构。而基于双目视觉的视觉里程计是利用每帧双目视觉获得的物体的三维特征点，通过匹配、跟踪等方式估计相机载体的短时位姿。视觉里程计在军用、民用的无人机中都有着广泛的应用。

在无GPS的环境下，基于视觉的无人机位姿估计作为一种低成本但是高效的手段，相比其他方法有着举足轻重的地位。在无人机的物资运送、森林火灾监视、人员救助、环境监测、航拍和军事侦查等任务中，视觉里程计一直担当着无人机的“眼睛”，确保无人机能在复杂的环境下时刻知晓自身的位姿从而完成任务。

因此研究鲁棒高效的无人机视觉里程计具有极其重要的战略意义。

发明内容

针对无人机对自身位姿短时估计的问题，本发明提出了一种基于双目视觉的半直接视觉里程计，一定程度上解决了双目视觉里程计位姿估计速度慢，依赖于深度估计精度的问题。

所述的基于双目视觉的半直接视觉里程计，具体步骤如下：

步骤一，利用双目视觉像机识别待检测区域，得到若干帧同时刻的左右图像；

左右图像采用带权重视差直方图与三线程图像分割得到。

步骤二，采用Sobel边缘检测与SAD匹配相结合，对各帧对应的左右图像进行粗探测，寻找左右图像的边缘像素最佳匹配点。

具体为：

首先，采用Sobel边缘检测算子剔除左右图像中的无关点，得到左右图像中包含静态物体的像素区域；

然后，针对左相机图像的每个边缘像素点A，利用极线约束剔除右相机图像中与该像素点A不在同一行的边缘像素点，将右相机图像中剩余的边缘像素点作为候选匹配像素点；

最后，对左相机图像每个边缘像素点，采用SAD匹配方法从右相机图像中与之对应的候选匹配像素点中，挑选出最佳的匹配点。

步骤三，利用双目视觉像机的参数，对完成粗探测的各帧同时刻左右图像提取视差直方图；

首先，针对每帧左右图像，将两个图像中匹配的各像素点对与双目视觉像机的参数进行匹配，得到多元集P；

多元集P中每个元素对应的是像素点匹配对，表示为{u_l,u_r,s}。

其中，u_l为每个像素点匹配对中左图像的x轴坐标，u_r为每个像素点匹配对中右图像的x轴坐标，s为像素点的物理尺寸。

然后，利用多元集P中每个元素的坐标计算各元素的视差d：

视差d：

d＝(u_r-u_l)*s

最后，利用左右图像的每个像素点匹配对的视差，作出视差直方图。

步骤四，根据视差直方图划分视差集中区域，并分配权重，筛选合并后得到静态物体区域。

具体为：

步骤401，将视差直方图改画为柱状形式，识别柱状图中的视差集中区域进行单独分割，并将分割出来的若干区域反投影回原视差直方图，得到对应的各静态物体的区域范围。

每个视差集中区域对应着一个静态物体；

步骤402，将所有视差集中区域的最高点为中心，按高斯分布为各区域块分配权重；

权重为w(d)～N(d_max,σ²)，d代表视差，d_max为该区域视差最密集处，σ为标准差；

权重越大代表属于静态物体区域的概率就越大。

步骤403，将所有视差集中区域连通，并进行筛选合并，得到所有静态物体的区域。

步骤五，对划分出静态物体区域的图像，从第一帧图像中提取ORB特征点，并利用光流法对视频流图像进行跟踪，判断连续跟踪特征点成功的帧数是否少于20帧，若是，则执行步骤六；若否，则执行步骤七。

图像特征点以左图像或右图像为基准，采用稀疏光流法进行跟踪右图像或左图像中各像素的运动位置。

步骤六，基于跟踪到的各帧图像中的特征点，通过直接法估计双目视觉像机的位姿；

具体为：

步骤601，利用相邻两帧图像中对应的特征点计算光度误差；

光度误差即两个像素之间的灰度误差，对于前一帧图像中某个特征点p₁，在当前帧图像中对应的特征点为p₂，则光度误差e为：

e＝I₁(p₁)-I₂(p₂)

I₁(p₁)为前一帧图像中特征点p₁的灰度，I₂(p₂)为当前帧图像中特征点p₂的灰度；

步骤602，对特征点进行第一次亚像素精度修正后，通过最小二乘法计算光度误差最小值，得到双目视觉像机的初始位姿；

亚像素精度修正是指：利用圆形图像块对特征点进行修正，剔除圆形图像块外跟踪偏离的特征点，保留剩下的特征点继续进行跟踪。

最小二乘问题为：

e_i＝w(d)_i*(I₁(p_1，i)-I₂(p_2，i))

J为最小二乘误差，特征点的总数为w个，则i＝1，2，3.....w，ξ为实际变换矩阵的李代数表示形式。w(d)_i为特征点i所在的静态物体区域的权重。

对最小二乘问题通过L-M法计算增量进行迭代求解，得到双目视觉像机的初始位姿。

步骤603，在得到最小化光度误差的基础上，对特征点进行第二次亚像素精度修正，计算最小化重投影误差，得到优化的双目视觉像机的位姿。

最小化重投影误差J′的最小二乘问题为：

其中，T为仿射矩阵，n代表当前帧图像中有效跟踪特征点的数量；

利用L-M法计算最小化重投影误差的最小二乘问题，得到优化的双目视觉像机的位姿。

步骤七，重新选择图像，使用特征点匹配法估计双目视觉像机的位姿。

首先，对双目视觉像机的获取的视频流中，选择新的未进行特征点匹配的相邻两帧图像g和h(h＝g+1)，提取ORB特征点进行重新匹配，并对特征点进行亚像素精度修正。

然后，利用局部光束平差法，构造3d-3d的迭代最近点(ICP)问题，即：

sign(scs)代表当前帧是否跟踪到像机待识别目标，是则此数为1，否则此数为0；f_j代表第j帧图像距离当前帧的帧数差；j、k、l为帧数，其中j、k∈{1，2，...，g}，l∈[j，...，g-1]且l∈N；i表示每个特征点，i∈[1，n]且i∈N。

最后，根据时间、有效跟踪特征点的数量和直方图中有效特征点的视差权重，这三个因素给每个仿射矩阵衍生的重投影误差分配权重，对最小重投影误差和通过最小二乘法进行优化，得到双目视觉像机的位姿。

本发明的优点为：

(1)开创性得将原先分离的深度估计和位姿估计过程耦合起来，相辅相承，在视觉里程计进行特征点提取前利用先前获得的跟踪区域分割出合适区域，充分利用了图像信息使得视觉里程计中特征点的提取、匹配与追踪更加快速且有效。

(2)改进传统的视差直方图，给每个像素除了坐标视差外引入第四属性权重，代表了深度精度的可信度，降低了区域边缘深度估计不准的影响，间接减少了双目视觉里程计精度对深度估计的依赖。

(3)首次将多种方法融合进行特征点跟踪，视具体情况采用相对最恰当的方法，并且在非极大值抑制、图像分割、图像块选取等细节上进行了优化调整，在位姿估计的速度与精度上都获得较好的性能。

(4)本发明考虑到了帧之间位姿的马尔可夫性，对于每帧位姿估计的代价，引入过去帧的代价进行综合，权重和计算方式与时间、有效跟踪点数量和有效跟踪点视差权重有关，将位姿信息和图像特征信息结合了起来。

附图说明

图1是本发明中多跟踪方法融合框架示意图；

图2是本发明基于双目视觉的半直接视觉里程计的计算流程图；

图3是本发明基于带权重视差直方图的静态物体区域探测方案流程图。

具体实施方式

下面结合附图，对本发明的具体实施方法进行详细说明。

本发明提出了一种基于双目视觉的半直接视觉里程计，在探测到图像中物体区域信息的基础上，融合了特征点匹配法、光流法与直接法进行特征点的跟踪以及无人机位姿的解算。各方法融合的方式如图1所示，其中，图像中物体区域信息是采用带权重视差直方图与三线程图像分割的方法得到的；特征点匹配法使用ORB特征进行帧之间特征点匹配，随后基于最小重投影误差与新的代价函数进行无人机位姿解算；光流法采用金字塔LK光流法进行特征点的跟踪与无人机位姿解算；直接法采用最小光度误差与新的代价函数进行特征点的跟踪与无人机位姿解算。

基于上述半直接的双目视觉里程计，无人机对自身的短时位姿进行估计，对图像信息的处理由浅入深，分别为探测图像中物体区域信息、特征点匹配或跟踪、位姿解算。

本发明的一种基于双目视觉的半直接视觉里程计，如图2所示，具体实施步骤如下：

步骤一，双目视觉像机识别待检测区域，得到左右图像；

步骤二，采用Sobel边缘检测与SAD匹配对左右图像进行粗探测，寻找左右图像的边缘像素最佳匹配点。

具体为：

步骤201，为了快速获得图像中静态物体可能出现的像素区域，采用Sobel边缘检测算子剔除左右图像中的大量无关点，得到左右图像中包含静态物体的像素区域，节约了随后双目相机左右图像像素点匹配的时间；

相比主流做法全局的处理每一帧，本发明的当前帧倘若非第一帧且未丢失跟踪目标，则利用先前帧已探测的物体区域进行边缘点提取，省去了大量无效匹配时间，将原本分离的深度估计和位姿估计部分耦合起来。

步骤202，在对剩余的边缘像素点进行匹配时，假设相机已经完美立体标定，针对左相机图像每个边缘像素点，利用极线约束再次剔除绝大部分右相机的边缘像素点。因为在相机完美立体标定下，静态物体的左右相机像素匹配点需在同一行。

步骤203，采用SAD匹配方法，在左相机图像每个边缘像素点和与之对应的剩余右相机图像边缘候选匹配像素点中，挑选出最佳的匹配点。

与传统双目相机像素点匹配算法不同，由于本发明针对无人机的短时位姿估计，只需要静态物体区域的视差或者深度信息，因此本发明的双目相机像素点匹配算法利用了物体边缘信息，更加快速有效是获得所需的像素点匹配对。在这一步仅仅是对静态物体区域的粗略估计，所以虽然Sobel边缘检测与SAD匹配算法均不是精度最好的算法，但是快速有效，且符合无人机算力有限的实际情况。

步骤三，利用双目视觉像机参数，提取完成粗探测的左右图像的视差直方图；

首先，从机载摄像头上提取机载相机参数，与拍摄所获视频流的左右图像，在进行匹配后，得到多元集P：{u_l，u_r，s}。

其中，u_l为视频流的左目每帧图像的任意一个像素点匹配对图像坐标系x轴坐标，u_r为视频流的右目每帧图像的任意一个像素点匹配对图像坐标系x轴坐标，s为像素的物理尺寸。

然后，计算多元集P中每个元素的视差d：

d＝(u_r-u_l)*s (1)

最后，对每个像素点匹配对应用公式(1)即可获得图像中物体边缘视差信息，并作出视差直方图。

步骤四，基于视差直方图分配权重，分割出左右图像中的静态物体区域。

如图3所示，具体为：

步骤401，将视差直方图改画为柱状形式，识别柱状图中的视差集中区域进行单独分割，并将分割出来的区域反投影回原视差直方图，得到静态物体的区域范围。

根据视差直方图，固定最高点对其进行拉伸，随后以4为单位宽度将视差直方图改画成柱状的形式以便于阈值分割。

由于静态物体的连续性，其视差分布也一定是连续的，因此视差直方图上会出现一定数量的视差集中区域，对应着静态物体的数量。

步骤402，对每块视差集中区域，以其中的最高点为中心，按高斯分布为其分配权重；

权重为w(d)～N(d_max，σ²)，d代表视差，d_max为该区域视差最密集处，σ为标准差；

本实施例将视差最密集处的权重设为1，取σ为此权重代表着越靠近中心的点，其确实属于静态物体区域的概率就越大，为之后的位姿估计做准备。

步骤403，对每块视差集中区域进行连通域检测，进一步细化静态物体的区域范围，得到连通区域，并用矩形框框出，得到的矩形框区域再进行筛选合并，即为所有静态物体区域。

为了提高检测速度，将图像按列分割成均分成三份，三线程同时进行连通域检测。在实际检测时，将每份图像左右适当进行扩展，使得三份图像彼此有重叠部分，避免了边缘效应带来的检测问题。

在获得分割后的图像的连通区域后，用矩形框框出。

去除面积过小的矩形框，再对邻近以及重叠的矩形框进行合并处理，判断邻近的条件如式(2)：

式中，rc1和rc2为某两个邻近的矩形1和矩形2在视差图中左上角顶点坐标(视差图原点默认在左上角，x轴向右，y轴向下)；width与height为矩形框的长和宽；dis为超参数，代表了可合并的矩形框的距离的上限。

最终可得到所有静态物体的矩形框区域。

步骤五，对划分出静态物体区域的左右图像，利用光流法检测图像环境，跟踪图像特征点，判断连续跟踪特征点成功的帧数是否少于20帧，若是，则执行步骤六；若否，则执行步骤七。

对视频流的第一帧图像提取ORB特征点(在本实施例中以左图像特征点为基准，实际选择左右图像不影响跟踪效果)，随后采用稀疏光流法进行跟踪。

稀疏光流法基于三大假设：

1.灰度不变假设，相邻图像中同一空间点的像素值不变。

2.运动一致假设，小窗口内所有像素运动相同。

3.小运动假设，极短时间内像素运动也很微小。

由此根据假设1，相邻两帧像素点灰度满足式(3)：

I₂(x+dx，y+dy，t+dt)＝I₁(x，y，t) (3)

I₁(x，y，t)和I₂(x，y，t)为前后两帧相应x，y坐标和时间t的灰度大小，dx，dy，dt为x，y轴与时间轴的细微变化量。

对式(3)的左边进行泰勒级数展开并且只保留一阶项，如式(4)：

由假设3，可以忽略式(4)的高阶项，即H.O.T，再结合式(3)，可得式(5)：

I_x为灰度在x方向上的偏导数，I_y为灰度在y方向上的偏导数，I_t为灰度关于时间的偏导数，V_x为像素在x方向上的速度，V_y为像素在y方向上的速度。

最后由假设2，一个长宽为dx、dy小窗口内的所有像素都满足式(5)，利用最小二乘法就可以跟踪图像中像素的运动，如式(6)：

其中，u、v分别为x、y轴上的运动速度，即dx/dt与dy/dt；k为窗口内所有需要跟踪的像素点数量。

在光流法引入图像金字塔的思想，利用八层图像金字塔，每层计算出的跟踪后像素位置将作为下一层的像素起始位置，直到最后一层输出像素的最终位置。由于图像的缩小，相对运动更加平缓，使得相邻图像间更能满足三大假设，跟踪效果更好。

根据最后的跟踪效果，如果稀疏光流法能大部分有效跟踪特征点，则说明上一帧特征点周围区域能基本符合三大假设，此时就可以运用基于相同假设的速度更慢但跟踪效果更好的直接法，否则使用特征点匹配法。因为稀疏光流法跟踪速度极快，相比其他两种方法用时几乎可以忽略不计，所以将它用作预判断的方法较为合理。

步骤六，通过直接法估计相机位姿；

具体为：

步骤601，基于步骤五中的假设，直接法通过最小化图像块的光度误差来比光流法更为准确得来获得相机位姿；

光度误差即两个像素之间的亮度误差，对于前一帧的某个特征点p₁，假设当前帧对应的点为p₂，则光度误差e为：

e＝I₁(p₁)-I₂(p₂) (7)

能对式(7)通过最小二乘法优化的前提正是灰度不变假设。

步骤602，对特征点进行第一次亚像素精度修正，剔除跟踪偏离的特征点，根据剩下的点重算图像间的仿射矩阵。

为了增加有效特征点数量，提高模型鲁棒性，现有技术中的主流做法往往会取特征点周围3*3图像块进行整体匹配，但是这忽略了一个问题：特征点在检测后一般会使用亚像素精度进行精度上调整，使得图像块中心特征点坐标为浮点数，而周围坐标为整数，导致可能出现实际上前后帧图像块并不匹配的问题。

针对此问题，本发明采用特殊的图像块，以为半径作圆，寻找在此圆内的特征点，并且对它们分别作亚像素精度修正，倘若有修正后出圆的，则剔除，保留剩下的特征点进行跟踪。

由于圆比方形更能体现以特征点为中心的思想，且在特征点坐标为浮点数时是完全契合的，更能有效跟踪特征点。

由于直接法不仅利用了图像灰度信息，同时也利用了特征点位置信息，因此精度比光流法更高，作为短时位姿估计精度较为可观。然而，虽然相比光流法，直接法不仅优化了灰度差而且优化了相机位姿，精度上更高，但是在灰度区分不大的环境依然会出现部分点的误跟踪，从而影响整体的位姿估计。因此在最小化光度误差后，需要删除那些跟踪明显偏离的特征点，根据剩下的点重算图像间的仿射矩阵，从而获得更优的位姿。

特别得，在赋予权重时，一个图像块内的特征点将被赋予与中心特征点相等的权重。

由于不同图像块内的特征点在带权重的视差直方图中被赋予了不同权重，因此在求解最小光度误差时，每个特征点的代价都将乘上其对应的权重，使得那些位于图像边缘的不太可信的点占总代价降低，将每个特征点以李代数的形式代表相机位姿，则可构成最小二乘问题，如式(8)：

J为最小二乘误差，即为光度误差最小代价。设特征点以及周围图像块内共w个点，则i＝1，2，3.....w，ξ为实际变换矩阵(下文中的仿射矩阵T)的李代数表示形式。

记上一帧所有点世界坐标到图像(像素)坐标的投影方程(9)：

式(9)中，P为特征点世界坐标，Z为特征点深度，K为相机内参阵。

记当前帧所有点世界坐标到图像(像素)坐标的投影方程为式(10)：

式(10)第一个等号后的式中T₁₂为上一帧到当前帧的仿射矩阵，也就是优化的目标，第二个等号后为其李代数表达形式。

采用非线性优化的办法，利用李代数左扰动模型在泰勒一阶展开后可得式(11)：

式(11)中u为扰动后当前帧像素点坐标，而由式(11)可知雅克比矩阵为下式(12)：

式(12)等式中为u处的像素梯度，第二项经推导可表示为式(13)：

f_x为相机在x轴上的焦距，f_y为相机在y轴上的焦距，XYZ为像素点u对应的世界坐标。

在获得了每个特征点的雅克比矩阵后，即可通过L-M法计算增量，迭代求解最小二乘问题，获得直接法下仿射矩阵的最优解从而获得相机位姿。

步骤603，在得到最小化光度误差的基础上，对特征点进行第二次亚像素精度修正，计算最小化重投影误差，得到优化的相机位姿。

由于已经获得了上一帧与当前帧的对应好的三维特征点组，因此此时求相机位姿的问题变成了一个3d-3d的迭代最近点(ICP)问题，但此时已经不是最小化光度误差，而是最小化重投影误差。同样的，相应代价也需乘以带权重视差直方图中算得的权重，如式(14)：

式中，J′为最小二乘法得到的重投影误差；

对式(14)同样利用非线性优化的办法和李代数左扰动模型，以位姿为优化变量，可得雅克比矩阵如式(15)：

同理，推导求得(16)：

利用L-M法就可以得到新的更优的相机位姿，同时确定了物体的准确区域，为下一帧的图像预处理提供了先验。

步骤七，重新选择图像，使用特征点匹配法估计相机位姿。

特征点匹配法不同于以上两点方法，它不需要基于三大假设，也放弃了直接跟踪上一图像帧特征点这一手段，而采用了直接在新图像上提取新的特征点并且通过匹配相邻两帧特征点来估计无人机的位姿的方案。这种方案诚然鲁棒性更好，精确率更高，但是计算代价远远超过了其他两种方法，因此特征点匹配法被作为在图像环境实在不符合三大假设时的选择方法。

在细节上，本方法通过提取相邻两帧的ORB特征点进行重新匹配，并采用亚像素精度进行坐标修正，由于在预处理阶段已经提前选定了特征点区域与限定了特征点只能为边缘点，因此提取特征点与匹配的速率大大提高，并且在跟距Hamming距离进行匹配后，无需传统常用的非极大值抑制与RANSAC筛选就已经能获得较好的性能。相比传统方法，由于本视觉里程计提前框定了特征点区域，最大化结合了图像信息，因此可以省去以上两个步骤，尤其省去RANSAC筛选将极大减小计算量。

随后利用局部光束平差法，通过构造3d-3d的迭代最近点(ICP)问题估计无人机短时位姿。在构造过程中同时考虑当前帧和之前数帧与前一帧的仿射矩阵，并且根据时间、有效点数量和步骤402中直方图中有效点的权重这三个因素给每个仿射矩阵衍生的重投影误差分配权重，最后优化最小重投影误差和。

在本发明的实施例中，以第六帧图像为例，即g＝5，h＝6在第六帧开始的位姿估计中，在当前帧位姿估计完成后，将当前帧与前五帧的代价联合起来进行综合优化，各自权重与时间、有效跟踪点数量和有效跟踪点视差权重有关，结合了之前获得的图像特征信息。

针对所有帧都有跟踪到的物体区域，以具体某一个为例，给予每个帧属性集C_i：{f_i，n_i，∑w(d)}。

其中f代表此帧代表距离当前帧的帧数上的距离，比如是当前帧的前一帧则f＝1。n代表此帧有效跟踪点的数量，∑w(d)所有有效跟踪点的权重和。

由于均已获得特征点的位置，因此以当前帧特征点为准，先匹配所有帧共同的匹配点，随后综合优化，实际上就是5个ICP问题的带权重的综合优化。代价如式(17)：

相比式(14)，式(17)根据时间、有效跟踪点的数量和有效跟踪点的权重和将每帧所有特征点的代价乘以了三个权重，分别为：与时间的倒数正相关，代表着越接近当前帧的帧，其算得的位姿越可靠；与有效跟踪点的数量正相关，代表此帧检测效果越好，虽然最后匹配时所用特征点数量相同，但是在那一帧倘若能跟踪更多特征点，说明其综合环境更适合跟踪；与有效跟踪点的权重和正相关，权重和越高说明此帧在带权重的视差直方图中的区域中所有点越接近中心，意味着其深度估计精度越高，间接说明了其位姿估计精度越高。

为了能使得问题优化同一个位姿矩阵，结合之前已获得的图像帧之间的位姿矩阵，跟踪帧将三个权重逐渐相乘作为常数并与优化位姿矩阵合并。特别的，符号sign(scs)代表着当前五帧中某一帧或某些帧是否跟踪到所需目标，是则此数为1，否则此数为0。在对雅克比矩阵的求解中，有与(15)、(16)类似形式，只需乘以相应常数即可。

相比传统方法，本方法虽然加上三个权重使得最小重投影误差更加复杂，但是只是增加了线性的计算量并且在此基础上使得位姿信息与图像特征信息结合，提高了视觉里程计的信息利用率，使得其能更大概率以及更快收敛到优化位姿解。

综合上述计算过程，本发明利用光流法检测图像环境，以判断使用特征点匹配法还是直接法进行特征点跟踪，如果使用直接法，则以最小光度误差与特殊的图像块构建最小二乘问题，而如果使用特征点匹配法，则以最小重投影误差构建最小二乘问题。针对最小二乘问题求解，提出了全新的代价函数。将过去几帧与当前帧的误差代价综合起来，权重和计算方式与时间、有效跟踪点数量与有效跟踪点视差权重有关。

本方法充分将视差直方图信息与位姿信息结合，使得最小重投影误差对于不同帧的权重分配更加合理，打破了传统方法将位姿信息与图像特征信息分离处理从而使得鲁棒性降低这个瓶颈。

Claims (4)

1.一种基于双目视觉的半直接视觉里程计，其特征在于，具体步骤如下：

首先，利用双目视觉像机识别待检测区域，得到左右图像，采用Sobel边缘检测与SAD匹配对左右图像进行粗探测，寻找各帧左右图像的边缘像素最佳匹配点；

然后，利用双目视觉像机参数，提取完成粗探测的左右图像的视差直方图，根据视差直方图划分视差集中区域，并分配权重，筛选合并后得到静态物体区域；

所述的视差直方图的提取方法，具体为：

首先，针对每帧左右图像，将两个图像中匹配的各像素点对与双目视觉像机的参数进行匹配，得到多元集P；

多元集P中每个元素对应的是像素点匹配对，表示为{u_l,u_r,s}；

其中，u_l为每个像素点匹配对中左图像的x轴坐标，u_r为每个像素点匹配对中右图像的x轴坐标，s为像素点的物理尺寸；

然后，利用多元集P中每个元素的坐标计算各元素的视差d：

视差d：

d＝(u_r-u_l)*s

最后，利用左右图像的每个像素点匹配对的视差，作出视差直方图；

所述的静态物体区域的划分过程为：

步骤401，将视差直方图改画为柱状形式，识别柱状图中的视差集中区域进行单独分割，并将分割出来的若干区域反投影回原视差直方图，得到对应的各静态物体的区域范围；

每个视差集中区域对应着一个静态物体；

步骤402，将所有视差集中区域的最高点为中心，按高斯分布为各区域块分配权重；

权重为w(d)～N(d_max,σ²)，d代表视差，d_max为该区域视差最密集处，σ为标准差；

权重越大代表属于静态物体区域的概率就越大；

步骤403，将所有视差集中区域连通，并进行筛选合并，得到所有静态物体的区域；

最后，对划分出静态物体区域的图像，从第一帧图像中提取ORB特征点，利用光流法跟踪视频流图像特征点，判断连续跟踪特征点成功的帧数是否少于20帧，若是，则基于光流法跟踪到的特征点，通过直接法估计双目视觉像机位姿；若否，则重新选择图像，使用特征点匹配法估计双目视觉像机位姿。

2.根据权利要求1所述的一种基于双目视觉的半直接视觉里程计，其特征在于，所述的寻找左右图像的边缘像素最佳匹配点的过程，具体为：

首先，采用Sobel边缘检测算子剔除左右图像中的无关点，得到左右图像中包含静态物体的像素区域；

然后，针对左相机图像的每个边缘像素点A，利用极线约束剔除右相机图像中与该像素点A不在同一行的边缘像素点，将右相机图像中剩余的边缘像素点作为候选匹配像素点；

最后，对左相机图像每个边缘像素点，采用SAD匹配方法从右相机图像中与之对应的候选匹配像素点中，挑选出最佳的匹配点。

3.根据权利要求1所述的一种基于双目视觉的半直接视觉里程计，其特征在于，通过直接法估计相机位姿，具体为：

步骤601，利用相邻两帧图像中对应的特征点计算光度误差；

光度误差即两个像素之间的灰度误差，对于前一帧图像中某个特征点p₁，在当前帧图像中对应的特征点为p₂，则光度误差e为：

e＝I₁(p₁)-I₂(p₂)

I₁(p₁)为前一帧图像中特征点p₁的灰度，I₂(p₂)为当前帧图像中特征点p₂的灰度；

步骤602，对特征点进行第一次亚像素精度修正后，通过最小二乘法计算光度误差最小值，得到双目视觉像机的初始位姿；

亚像素精度修正是指：利用圆形图像块对特征点进行修正，剔除圆形图像块外跟踪偏离的特征点，保留剩下的特征点继续进行跟踪；

最小二乘问题为：

e_i＝w(d)_i*(I₁(p_1,i)-I₂(p_2,i))

J为最小二乘误差，特征点的总数为w个，则i＝1,2,3.....w，ξ为实际变换矩阵的李代数表示形式；w(d)_i为特征点i所在的静态物体区域的权重；

对最小二乘问题通过L-M法计算增量进行迭代求解，得到双目视觉像机的初始位姿；

步骤603，在得到最小化光度误差的基础上，对特征点进行第二次亚像素精度修正，计算最小化重投影误差，得到优化的双目视觉像机的位姿；

最小化重投影误差J'的最小二乘问题为：

其中，T为仿射矩阵，n代表当前帧图像中有效跟踪特征点的数量；

利用L-M法计算最小化重投影误差的最小二乘问题，得到优化的双目视觉像机的位姿。

4.根据权利要求1所述的一种基于双目视觉的半直接视觉里程计，其特征在于，所述的特征点匹配法估计相机位姿，具体为：

首先，对双目视觉像机的获取的视频流中，选择新的未进行特征点匹配的相邻两帧图像g和h，h＝g+1，提取ORB特征点进行重新匹配，并对特征点进行亚像素精度修正；

然后，利用局部光束平差法，构造3d-3d的迭代最近点(ICP)问题，即：

sign(scs)代表当前帧是否跟踪到像机待识别目标，是则此数为1，否则此数为0；f_j代表第j帧图像距离当前帧的帧数差；j、k、l为帧数，其中j、k∈{1,2,…,g}，l∈[j,…,g-1]且l∈N；i表示每个特征点，i∈[1,n]且i∈N；

最后，根据时间、有效跟踪特征点的数量和直方图中有效特征点的视差权重，这三个因素给每个仿射矩阵衍生的重投影误差分配权重，对最小重投影误差和通过最小二乘法进行优化，得到双目视觉像机的位姿。