基于半直接法的单目视觉里程计定位方法及定位系统与流程

本发明属于三维重建中视觉里程技术领域，具体来说是一种基于半直接法的单目视觉里程计定位方法及定位系统。

背景技术

随着近几年计算机视觉和图像处理等技术的不断演进，视觉传感器在越来越多的场景得以应用，基于视觉传感器的里程计也得到越来越多的研究者的关注。视觉传感器以其成本低廉、信息丰富等优点正在逐步成为里程计应用的主要传感器之一。与基于slam(同步定位与建图)相比，视觉里程计的效率更高、对硬件要求更低。传统的轮式里程计主要是通过轮子的滚动来进行里程计算的，但是轮子在崎岖的路面上可能出现打滑、空转的情况，就会给里程计测量带来较大的误差，而基于视觉传感器的里程计则不会受此影响。

目前主流的视觉里程器按硬件来分，大致可分为只使用一个相机的单目系统和使用立体相机的双目系统两类。单目是指仅使用一个相机作为唯一传感器，而双目是指使用一个立体相机，在每个时刻同时采集两幅图像。与双目相比，单目视觉里程器具有成本低、无需考虑相机基线对系统精度的影响等优点。

在2014年，christianforster提出的一个结合了直接法和特征点法的特点的一种视觉里程计，名为svo。svo首先对图像提取fast特征点，但是并不计算特征点的描述子，而是利用直接法估计一个相机的初始位姿，节约了大量特征检测和匹配的时间，同时仅对fast特征点进行计算，使其速度飞速提升，非常适用于无人机、智能手机等计算资源有限的平台。但是svo因为过于追求极致的速度，而导致鲁棒性不足。

技术实现要素：

本发明旨在至少解决现有技术中存在的技术问题，特别创新地提出了一种基于半直接法的单目视觉里程计定位方法及定位系统。

为了实现本发明的上述目的，根据本发明的第一个发明，本发明提供了一种基于半直接法的单目视觉里程计定位方法，其包括如下步骤：

s1，利用无人机或车载的单目相机进入目标场景并记录场景图像数据；

s2，对步骤s1获得的场景图像进行预处理，畸变校正并对校正后的图像进行特征点检测；

s3，系统初始化，跟踪步骤s2获得的特征点，将跟踪得到的特征点对集合作为鲁棒性算法的数据集，利用八点法估计两幅图像的基础矩阵f和利用四个点对计算单应性矩阵h，通过计算两个矩阵的内点对选择基础矩阵f和单应性矩阵h之一进行后续计算；

s4，对相机进行跟踪定位，形成相机的运动轨迹，提取场景图像中的线段特征，使用半直接法进行相机的姿态估计，首先，将稀疏图像对齐，通过当前帧图像和上一帧图像上的特征点建立光度误差，求得一个初略的相机姿态；然后，特征对齐，将地图中的所有地图点利用上一步求得的相机姿态投影到当前帧上，利用投影点和地图点在参考帧上的灰度差建立光度误差，优化投影点在图像上的位置；最后，利用优化后的位置和投影点建立关于三维点和相机姿态的重投影误差，对相机姿态和三维点进行优化，得到当前帧最终的相机姿态；

s5，选取相机拍摄图像中的关键帧；

s6，地图构建，对关键帧中的线段特征的端点进行深度滤波，从新获取的关键帧中新建一个种子点，利用距离新获取的关键帧最近的几个关键帧对其进行一次预更新，再将其加入到种子点队列里面，如果种子点队列收敛，则新建一个关键帧，把地图中所有的三维点投影到新的关键帧上，如果三维点在新关键帧上可见，就将该三维点的参考帧更改为新关键帧，如果种子点队列不收敛，则继续获取关键帧，更新种子点队列。

为了实现本发明的上述目的，根据本发明的第二个发明，本发明提供了一种基于半直接法的单目视觉里程计定位系统，其包括单目视觉相机和处理器，所述单目视觉相机有无人机携带或车辆携带，所述单目视觉相机采集图像并传输给处理器，所述处理器利用本发明所述的方法进行处理，并构建出地图，定位并在地图上显示相机的运动轨迹。

本发明采用视觉里程器来测量里程可以降低里程测量的误差。而传统的轮式里程计主要是通过轮子的滚动来进行里程计算的，但是轮子在崎岖的路面上可能出现打滑、空转的情况，就会给里程计测量带来较大的误差，而基于视觉传感器的里程计则不会受此影响。

本发明采用了单目视觉里程器，与双目视觉里程器，甚至多目视觉里程器相比，具有成本低、无需考虑相机基线对系统精度的影响等优点。

本发明实现了鲁棒、高质量的初始化的过程。新的关键帧选取策略，增强改善其基于场景深度的关键帧选取策略，消除了只能用于相机下视的限制。

本发明在每个三维特征首次加入到深度滤波器是时选取关键帧对该三维特征的深度信息进行一次更新，降低新三维特征深度信息的不确定性，加快深度的收敛；对三维特征的参考帧进行更新，缩短参考帧与当前帧的时间间隔，降低对光照变化的敏感程度。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是本发明一种优选实施方式中基于半直接法的单目视觉里程计定位方法的流程图；

图2是本发明一种优选实施方式中畸变校正效果图，其中图2(a)为校正前的图像，图2(b)为校正前的图像；

图3是本发明一种优选实施方式中初始化流程示意图；

图4是本发明一种优选实施方式中鲁棒估计示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

本发明提供了一种基于半直接法的单目视觉里程计定位方法，如图1所示，其包括如下步骤：

s1，利用无人机或车载的单目相机进入目标场景并记录场景图像数据；

s2，对步骤s1获得的场景图像进行预处理，畸变校正并对校正后的图像进行特征点检测；

s3，系统初始化，跟踪步骤s2获得的特征点，将跟踪得到的特征点对集合作为鲁棒性算法的数据集，利用八点法估计两幅图像的基础矩阵f和利用四个点对计算单应性矩阵h，在本发发明另外的优选实施方式中，还可以用非线性优化之类的方法计算，通过计算两个矩阵的内点对选择基础矩阵f和单应性矩阵h之一进行后续计算；

s4，对相机进行跟踪，提取场景图像中的线段特征，使用半直接法进行相机的姿态估计，首先，将稀疏图像对齐，通过当前帧图像和上一帧图像上的特征点建立光度误差，求得一个初略的相机姿态；然后，特征对齐，将地图中的所有地图点利用上一步求得的相机姿态投影到当前帧上，利用投影点和地图点在参考帧上的灰度差建立光度误差，优化投影点在图像上的位置；最后，利用优化后的位置和投影点建立关于三维点和相机姿态的重投影误差，对相机姿态和三维点进行优化，得到当前帧最终的相机姿态；

s5，选取相机拍摄图像中的关键帧；

s6，地图构建，对关键帧中的线段特征的端点进行深度滤波，从新获取的关键帧中新建一个种子点，利用距离新获取的关键帧最近的几个关键帧对其进行一次预更新，再将其加入到种子点队列里面，如果种子点队列收敛，则新建一个关键帧，把地图中所有的三维点投影到新的关键帧上，如果三维点在新关键帧上可见，就将该三维点的参考帧更改为新关键帧，如果种子点队列不收敛，则继续获取关键帧，更新种子点队列。

在本实施方式中，相机的成像畸变径向畸变和切向畸变两个部分组成，其畸变系数用一个5维的向量(k1,k2,k3,p1,p2)来表示。k3的影响较小，在本发明中，它不参与运算。相机的畸变系数可以在相机标定的过程中进行求解，畸变系数和相机内参一样，仅与相机的硬件相关。在畸变校正过程中，畸变图像是已知的，需要求的是非畸变图像，通常的做法是对于给定非畸变图像的坐标(u,v)，然后计算这个点在畸变后应处于图像畸变图像的坐标(ud,vd)，这样就得到了非畸变图像坐标(u,v)的像素值，遍历所有畸变校正后的图像的坐标之后就得到了完整的畸变校正后的图像。

畸变校正的方法为：

根据给定的像素坐标求解其在图像平面上的物理坐标(x',y')：

畸变之后的物理坐标为：

r²＝x'²+y'²

将畸变后的物理坐标转换为图像上的像素坐标：

其中，(k1,k2,k3,p1,p2)为相机的畸变系数，(u,v)为非畸变图像的坐标，(ud,vd)为(u,v)对应的畸变图像的坐标，(cx,cy)是相机光心的位置；(fx,fy)是相机x方向和y方向的焦距。

图2中，左边是畸变校正前的图像，右边是畸变校正后的效果图。图中的红线在现实世界中是一条直线，然而因为畸变的关系，左图已经发生了较为严重的弯曲，通过畸变校正后，右图中的红线位置校正为了直线。

在本实施方式中，特征点检测的方法为：

以图像中的一个像素点为圆心，j为半径建立一个圆，通过圆上的像素点和圆心点的差异来判断像素点是否为特征点，其中j为大于1的正整数；若圆心点p的灰度值和圆上任意连续的k个像素的灰度值之差都同大于或同小于阈值，则点p就是一个特征点，所述k的数值为j*j。在本发明中，优选j为3。上述方法在图像中的边缘会的到大量的无用特征点，因此在初次检测完成后对所有检测出来的特征点执行非极大值抑制，选取区域中最好的特征点。

在本实施方式中，如图3所示，初始化具体包括：

s31，判断图片中的特征点的个数，如果特征点个数超出阈值(例如100)，则判断是否已经存在参考帧(即第一个满足超出特征点阈值的图像)，如果有参考帧，则执行步骤s32，如果没有参考帧，则将该图片保存为参考帧，执行步骤s32，当特征点的个数没有超出阈值时，等待输入下一个场景图片；

s32，对特征点进行跟踪，对于连续两个时刻t，t+1获取的灰度图像it，it+1，假设it中的一个待跟踪的窗口w，窗口w内的像素在两幅图像中的位置满足：

it(x,y)＝it+1(x+δx,y+δy)，

通过最小化待跟踪窗口在图像间的灰度差平方和寻找待跟踪窗口的匹配位置：

其中，e(w)为待跟踪窗口的匹配位置，

对上式进行泰勒展开至高阶线性项，分别令偏导数为零便可求得极小值，求出待跟踪窗口在图像间的运动(δx,δy)；

s33，判断跟踪效果，如果满足阈值要求(跟踪成功的特征点数量大于50)，则执行步骤s34，否则判断该帧是否与参考帧间隔超过50帧，如果超过则删除参考帧，重新输入图片，如果没有超过50帧，则重新输入图片；

s34，如图4所示，在两幅图像中匹配上的特征点对中随机选取计算目标模型所需要最少的数据数目(4对)，利用选出的最小数据产生模型，模型为利用八点法估计两幅图像的基础矩阵f或者利用四个点对计算的单应性矩阵h，通过这个模型去适应数据集中的数据，符合当前模型的就记为本次迭代的内点，否则就记为外点，通过内点所占的比例来评价当前模型的好坏，记录内点比例最高的模型(内点的占比最高)，如果已满足迭代结束条件，则结束，否则返回本步骤最开始，所述迭代结束条件包含两个部分：第一个部分是达到最大迭代次数；第二个部分是设置一个迭代次数k，k随着每次迭代而更新，其更新公式如下：

其中，p为置信度，w是当前最优模型下的内点的比例，m是拟合模型缩小的最小的数据数量，当迭代次数大于k或者最大迭代次数中的任意一个都停止迭代；

利用八点法估计两幅图像的基础矩阵f和利用四个点对计算单应性矩阵h，估计基础矩阵时，最小子集为随机选取的8个点对，而估计单应性矩阵时最小子集为随机选取的四个点对；

对同时估计出来的基础矩阵f和单应性矩阵h进行评价，对于估计出来的模型m，在特征点对集p下的得分可表示为sm：

其中(pi,pi')表示一对跟踪成功的特征点，d²(p,p',m)表示在模型m下，两个点的对称转移误差，ρm(d²(p,p',m))表示对d²(p,p',m)进行卡方检验来确认这对点对是否是内点对，最后内点对的数量就是该模型的最终得分，用这种方式计算基础矩阵的得分sf和单应性矩阵的得分sh：

如果r>0.4，那么选择单应性矩阵h，否则就选择基础矩阵f。

在本实施方式中，所述步骤s4具体包括：

s41，用lsd算法(具体的算法参考论文"lsd:alinesegmentdetector")提取场景图像的线段特征，对提取出来的线段特征进行均匀采样，转化为点特征；

s42，对于新到达的图像帧进行特征点提取作为特征点，稀疏图像对齐，通过当前帧图像和上一帧图像上的特征点建立光度误差，求得一个初略的相机姿态；特征对齐，将地图中的所有地图点利用上一步求得的相机姿态投影到当前帧上，利用投影点和地图点在参考帧上的灰度差建立光度误差，优化投影点在图像上的位置；最后，利用优化后的位置和投影点建立关于三维点和相机姿态的重投影误差，对相机姿态和三维点进行优化，得到当前帧最终的相机姿态；

其中，稀疏图像对齐指：

假定当前时刻为k，当前图像帧为ik，上一时刻为k-1，上一图像帧为ik-1,此时是求解相邻两帧的相对姿态，点pi为图像ik-1上任一特征点，其反投影到三维空间在其相机坐标系下的点为pi，将pi投影到ik上得到的投影点为pi'，则光度误差为：

e＝ik-1(pi)-ik(pi')

对于线段特征，在线段上进行均匀采样，将前一帧图像上的线段特征反投影到三维空间再投影到当前帧；

线段特征的光度误差是其采样点光度误差之和，若相机内参矩阵为k，当前帧相对上一帧的相机姿态为ξ，则：

pi'＝kexp(ξ^)pi，

每次迭代求解一个相机姿态的增量，使得整个代价函数的值减小直到满足停止迭代的条件，给ξ左乘一个小扰动δξ：

将上式进泰勒展开到一阶并化简可得：

上式中，是点pi'处的梯度，为投影方程关于相机姿态的导数，由链式法则：

其中，pi'＝exp(ξ^)pi是相机坐标系下的三维点，记其坐标为(x',y',z')，则：

而为pi'关于相机姿态的导数，根据式：

因此：

那么，在稀疏图像对齐阶段，使用光度误差简历代价函数的雅克比矩阵为：

利用式不断的迭代，降低总的误差，最终就可以求得前一时刻和当前时刻时间相机的相对姿态，进而累积得到当前相机在世界坐标系下的相机姿态；

特征对齐包括：将地图中所有在当前图像中中可见的三维点利用稀疏图像对齐求得的相机姿态投影到图像上，得到一系列的投影点，对于地图中的三维点pi(x,y,z)，第一次观测到它的关键帧称为其参考关键帧，记其参考关键帧的图像为iref，其在参考帧中的位置为pi，该三维点在时刻k的图像上可见且其投影点为ui，采用光度误差：

e＝iref(pi)-ik(ui),

特征对齐的待优化参数是投影点的位置ui，

线段在三维空间由其两个端点表示，在特征对齐阶段，将三维线段的端点分别投影到当前图像中，进而对齐其端点的位置即可完成对线段特征的对齐，

最后，在姿态和三维结构优化阶段，使用特征对齐阶段投影点优化后的位置和未优化的位置之间的距离建立重投影误差，对稀疏对齐阶段求得的相机姿态和地图中的三维点位置进行优化，假定地图中的任一三维点pi的投影点ui，假定其优化后的位置为ui'，则：

e＝u′i-ui,

对线段特征而言，其重投影误差由其两个端点构成，由李代数的扰动模型：

其中，pi'表示三维点pi利用相机坐标系下的三维点，上式实际上就是表示投影方程对相机姿态的求导，对三维点进行优化时，需要了解e随着三维点的变化如何变化，分析e关于三维点pi'的导数，由链式求导法则：

假定当前相机姿态为r，t，则由投影方程：

pi'＝rpi+t,

pi'对pi求导实际上就是旋转矩阵r，重投影误差相对于三维点的雅克比矩阵为：

将重投影误差关于姿态的雅克比矩阵和关于三维点的雅克比矩阵组合到一起便是姿态和结构优化过程中迭代的雅克比矩阵。

在本实施方式中，所述步骤s5具体包括：

在以下三种情况中满足一种便开始选择关键帧：

①地图构建线程较为空闲时，这主要分为两个方面，一是待处理关键帧队列为空，同时在地图构建线程中，待更新的三维点的数量较少，此时考虑添加关键帧来提升地图的稠密度；

②距离当前帧最近的关键帧上的特征点在当前图像帧上的可见度较低，距离当前帧的关键帧和当前帧应有较大的场景重合，发生此情况时说明很大可能相机正在进入一个新的区域，需要立即添加关键帧保证相机跟踪的正确进行；

③当前图像和上一帧图像相对姿态中的位移部分超过了场景平均深度的一定比例；

一个图像帧还需要满足如下条件才可以被选为关键帧：

①与最近关键帧的距离在一定范围内；(0.5—2，这个只是人为设定的，跟采用的数据及有关，所以没有固定的标准)

②在特征对齐阶段，可重投影到当前图像上的特征点的三维点的数量和上一帧相比不低于一定值(40％)；

关键帧选取算法为：

输入当前图像帧，上一图像帧；

如果关键帧队列为空，且如果未收敛的三维点<100，则伪代码needkfs＝true，开始选择关键帧，投影关键帧上的所有三维点到当前图像帧上，如果当前帧可见的三维点/关键帧上所有三维点<0.5，则needkfs＝true，

如果当前帧的位移/场景平均深度>0.1，则needkfs＝true，

如果needkfs＝true，则计算当前帧与关键帧的距离，

如果当前帧与关键帧的距离<小于最大距离阈值并且当前帧与关键帧的距离>最小距离阈值，则将地图中的所有点投影到当前图像帧上，可见数量为m，将地图中的所有点投影到上一图像帧上,可见数量为n，

如果m/n>＝0.4，则当前帧是关键帧。

在本实施方式中，所述步骤s6具体包括：

对线段特征的端点进行深度滤波，当深度滤波器获得一个关键帧，将关键帧上的特征点初始化为种子点并放入种子队列，每当深度滤波器获得一个普通图像帧，利用该图像帧来更新种子队列的所有种子点，即更新这个种子点的深度值和深度的不确定性值，种子点在新图像上位置不确定但在一条直线(极线)上；

依据种子点在参考帧中的特征点u在当前图像上的极线进行搜索，获得对应点u'(极线匹配)，在搜索的时候取特征点领域内的像素作为模板进行模板匹配；

在获得匹配点之后利用三角测量计算深度；利用贝叶斯概率模型对原有的深度估计和不确定性进行更新，若更新后深度信息收敛则加入到地图中，若更新后深度信息发散了则删除该种子点，否则就等待下一次更新；

深度预更新，从新的关键帧中新建一个种子点，利用距离新关键帧最近的几个关键帧对其进行一次预更新，然后再将其加入到种子点队列里面，具体深度预更新步骤为：

获取新的关键帧；

获取新的关键帧的平均深度；

找到距离新的关键帧最近的n个关键帧，

对于新的关键帧中的特征点ft：

使用特征点ft新建一个种子点，初始化深度为平均深度；

对于所述n个关键帧中的关键帧kf:

与新的关键帧进行极线搜索；

三角测量计算特征点ft的深度和不确定性，并基于贝叶斯概率模型对种子点进行更新；具体计算方法可以采用“remode:probabilistic,monoculardensereconstructioninrealtime”这篇论文里面描述的方法；

将种子点加入到种子点队列

输出新的种子点队列；

每次新建一个关键帧，便把地图中所有的三维点投影到新的关键帧上，如果三维点在新关键帧上可见，就将该三维点的参考帧更改为新关键帧，将图像中的所有三维点投影到最新的关键帧中，用投影点在最新图像中的灰度和对应三维点在参考帧中的灰度建立光度误差，优化投影点在最新图像帧之间的位置，构建出地图。

需要说明的是，本发明中基础矩阵f、单应性矩阵h和旋转矩阵r的计算方法采用现有的计算方法，相机内参矩阵k的获取方法采用本领域通用的获取方法。

本发明还提供了一种基于半直接法的单目视觉里程系统，其包括单目视觉相机和处理器，所述单目视觉相机有无人机携带或车辆携带，所述单目视觉相机采集图像并传输给处理器，所述处理器利用本发明所述的方法进行处理，并构建出地图。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。