CN114494150A - 一种基于半直接法的单目视觉里程计的设计方法 - Google Patents

Abstract

本发明公开了一种基于半直接法视觉里程计的设计方法，包括如下步骤：S1、通过相机采集图像数据，获得图像序列；S2、对单目相机初始化，确定其尺度；S3、采用稀疏图像对齐最小化两个像素块之间的光度误差，获取粗略的相机位姿，以及像素点之间的对应关系；S4、进行ORB特征提取和BRIEF描述子的计算，优化相机的位姿；S5、将三维地图点与当前帧的特征点进行匹配，利用最小化重投影误差进一步优化当前位姿；S6、输出相机位姿；S7、判断是否有新图像加入，若是则执行步骤S3，进行算法的循环，否则结束流程；本发明的方法处理速度快，定位精度高，能够处理稀疏纹理、光照变化以及移动物体等难题。

Description

一种基于半直接法的单目视觉里程计的设计方法

技术领域

本发明涉及视觉定位SLAM技术领域，特别是涉及一种基于半直接法的单目视觉里程计的设计方法。

背景技术

目前移动机器人、自动驾驶、无人车和增强现实等技术受到人们越来越多的关注，同步定位与地图构建(SLAM)技术在其中发挥着重要作用。随着移动机器人的应用场景不断扩大，所面临的问题也越来越复杂，这对于移动机器人的定位技术提出了要求。

视觉SLAM是由传感器数据、前端视觉里程计(VO)、后端非线性优化、回环检测和构建地图这五部分构成，其中视觉里程计在视觉SLAM***中被称为前端，它关心的是相邻图像之间的运动，机器人通过所携带的视觉传感器获取连续的图像数据，利用图像上的信息以及相机的模型来估计相机位姿的变化，并将待优化的初始数据提供给后端进行优化。依据所用传感器的类型可以分为单目、双目、RGB-D；从实现的形式上可以分为直接法和特征点法。其中，直接法是利用像素之间的信息进行计算，最小化光度误差，它的优点是计算速度比较快，但容易受光照的影响。特征点法是通过将图像抽象为特征点的集合，利用描述子作为数据关联的依据，具有一定的旋转和尺度不变性，对光照不敏感，因此，特征点法的这些优良特性使得其在大多数场景下具有很好的鲁棒性，也已经成为主流的前端方法，但其存在一些问题，比如特征点的提取和描述子的计算量比较大，一幅图像当中包含的信息非常丰富，特征点只占图像像素的一小部分，因此丢弃了大部分可能具有有用信息的图像像素，倘若场景当中没有足够的特征点。比如一面白墙或者地板等等，这将会导致特征点法跟踪失败。为了解决上述问题，本文发明了一种基于半直接法的单目视觉里程计的设计方法。

发明内容

本发明的目的是解决现有技术的不足，提供一种基于半直接法的单目视觉里程计的设计方法。

为了解决上述问题，本发明采用如下技术方案：

一种基于半直接法的单目视觉里程计的设计方法，包括如下步骤：

S1：通过相机采集图像数据，获得图像序列，并传输到分析设备上；其中分析设备为电脑，相机为单目相机；

S2：通过单目初始化流程，确定图像的尺度；

S3：基于直接法估计相机的初始位姿；

S4：基于特征点法优化相机的初始位姿；特征点法包括对图像进行ORB特征提取和特征匹配的过程；

S5：通过将三维地图点与当前帧的特征点进行匹配，利用最小化重投影误差进一步优化当前相机位姿；

S6：输出相机位姿，包括三维坐标点、平移向量和旋转向量；

S7：判断是否有新图像加入；若是则返回S3，进入算法的循环；否则结束流程。

进一步的，所述S2中的单目初始化流程包括如下步骤：

S21：移动相机，并获取图像，组成图像序列；其中，移动相机时，要求在设定方向上移动超过设定值的距离；

S22：选取图像序列中的前一帧作为初始帧，后一帧作为参考帧；

S23：通过对初始帧图像和参考帧图像进行ORB特征点提取和特征匹配，建立两帧图像之间的数据关联；

S24：利用三角测量得到初始帧中特征点的深度；其中三角测量表示通过两处观察同一个点的夹角，从而确定该点的距离。

进一步的，所述S23中的数据关联的建立，首先需要设定任意参考空间点P，空间点P在两帧图像中对应的投影点像素坐标分别为p₁，p₂；K为相机的内参，P为空间点P的世界坐标,设定初始帧到参考帧的相机运动变换为旋转矩阵R，平移向量t；然后通过对极几何约束求出本质矩阵E；最后使用八点法对求得的本质矩阵E进行奇异值分解，获得旋转矩阵R，平移向量t。

进一步的，所述对极几何表达式如下：

p₂ ^TK^-Tt^RK^-1p₁＝0 (1)

E＝t^R (2)

在已经获得R和t的基础上，设定两个特征点的深度为s₁，s₂；其中深度s₁，s₂满足如下公式：

s₁x₁＝s₂Rx₂+t (3)

其中，x₁和x₂表示两个特征点R和t归一化后的坐标；最后通过最小二乘法进行求解出s₁和s₂。

进一步的，所述S3中的初始位姿估计过程为采用稀疏图像对齐，最小化两个像素块之间的光度误差获取粗略的相机位姿，以及像素点之间的对应关系。

进一步的，所述初始位姿估计过程具体包括：

首先设定参考空间点P，参考空间点P在初始帧图像和参考帧图像中对应的投影点像素坐标分别为p₁，p₂；则空间点P在两帧图像中的像素光度误差e为：

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

其中，I₁(p₁)表示初始帧图像中像素坐标为p₁的像素光度；I₂(p₂)表示参考帧图像中像素坐标为p₂的像素光度；通过像素光度误差e获得优化一个空间点P的光度误差的二范数为：

min_ξJ(ξ)＝‖e‖² (4)

其中，ξ表示相机位姿的李代数；J(ξ)表示相机位姿；

考虑空间中N个像素点的光度误差e_i＝I₁(p_1,i)-I₂(p_2,i),i∈(1,N)，则N个像素点的相机的位姿估计表示为：

求式(5)中的优化问题，需要通过分析像素光度误差e和相机位姿的李代数ξ的导数关系；其中通过使用李代数的扰动模型，给exp(ξ)左乘一个设定的小扰动exp(δξ)获得：

其中，K为相机的内参；P为空间点P的世界坐标；Z₁、Z₂分别表示；设定q为扰动分量在第二个相机坐标系的三维坐标，u为扰动分量的像素坐标，获得：

q＝δξ^exp(ξ^)P (7)

将式(7)、(8)代入式(6)，并对式(6)进行一阶泰勒公式展开，获得：

其中，

表示u处的像素梯度，

表示投影方程关于相机坐标系下的三维空间点的导数，

表示变换后的三维点对的导数；δξ通过设定的小扰动exp(δξ)获得；

最后推导出单个点误差相对于李代数ξ的雅克比矩阵；并通过列文伯格-马夸尔特方法进行增量计算，获得N个像素点的光度误差，迭代求解相机位姿J，表示为：

进一步的，所述S4中，基于特征点法优化相机的初始位姿，包括如下步骤：

S41：首先需要将图像分成若干大小相等的网格，对每个网格进行Oriented FAST角点提取，使用四叉树分配算法均匀化特征点；

S42：构建直方图，筛选出与主流方向一致的特征点作为候选匹配点；

S43：对保留下来的特征点进行BRIEF描述子的计算；其中，首先找出所有描述子之间的最小距离和最大距离，当描述子之间的距离小于两倍最小距离时，则匹配成功，否则为误匹配；描述子之间的距离表示两个特征点的相似程度；

S44：采用随机采样一致性RANSAC算法剔除掉大量误匹配的特征点；

S45：使用对极几何约束获得新的本质矩阵E2，并通过八点法对求得的本质矩阵E2进行奇异值分解，得到相机的位姿R2和t2；R2为旋转矩阵，t2为平移向量。

进一步的，所述S5中，将三维地图点与当前帧的特征点进行匹配，利用最小化重投影误差优化当前位姿，具体包括如下过程：

首先设定空间点坐标P_i＝[X_i,Y_i,Z_i]^T，在当前帧中的像素坐标为u_i＝[u_i,v_i]^T,s_i为该点的深度，相机的位姿采用李代数的表示形式ξ；依据相机的针孔模型可得像素位置与空间点P的对应关系有：

转换为矩阵形式，表示为：

s_iu_i＝Kexp(ξ^)P_i (12)

在等式(12)存在一个误差，通过对误差求和，并构建最小误差函数，能够求解出最优的相机位姿ξ^*，表示为：

对于式(13)中的非线性优化问题，可以再使用高斯牛顿法或者列文伯格-马夸尔特方法进行求解，获得最优相机位姿ξ^*。

本发明的有益效果为：

通过将直接法与特征点法相结合，其中首先采用稀疏图像对齐进行初始位姿的估计，再通过ORB特征提取和匹配进一步优化当前位姿，最后通过局部三维地图点的匹配来提高特征点定位的准确性，减少了定位过程的计算量，并且优化了光度误差，确保精确度；

本发明的计算处理速度比较快，定位精度高，能够处理稀疏纹理、光照变化、移动物体等难题。

附图说明

图1为本发明实施例一的流程图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

实施例一：

如图1所示，一种基于半直接法的单目视觉里程计的设计方法，包括如下步骤：

S1：通过相机采集图像数据，获得图像序列，并传输到分析设备上；在本例中分析设备为电脑，相机为单目相机；

S2：通过单目初始化流程，确定图像的尺度；

S3：基于直接法估计相机的初始位姿；

S4：基于特征点法优化相机的初始位姿；特征点法包括对图像进行ORB(Orientedand Rotated BRIEF)特征提取和特征匹配的过程；

S5：通过将三维地图点与当前帧的特征点进行匹配，利用最小化重投影误差进一步优化当前相机位姿；其中三维地图点表示空间坐标点；

S6：输出相机位姿，包括三维坐标点、平移向量和旋转向量；

S7：判断是否有新图像加入；若是则返回S3，进入算法的循环；否则结束流程。

所述S2中的单目初始化流程包括如下步骤：

S21：移动相机，并获取图像，组成图像序列；在本例中，移动相机时，要求在设定方向上移动超过设定值的距离；

S22：选取图像序列中的前一帧作为初始帧，后一帧作为参考帧；

S23：通过对初始帧图像和参考帧图像进行ORB特征点提取和特征匹配，建立两帧图像之间的数据关联；

S24：利用三角测量得到初始帧中特征点的深度；其中三角测量表示通过两处观察同一个点的夹角，从而确定该点的距离。

所述S23中的数据关联的建立，首先需要设定任意参考空间点P，空间点P在两帧图像中对应的投影点像素坐标分别为p₁，p₂；K为相机的内参，P为空间点P的世界坐标,设定初始帧到参考帧的相机运动变换为旋转矩阵R，平移向量t；然后通过对极几何约束计算两帧之间的位姿变换；最后使用八点法对求得的本质矩阵E进行奇异值分解，获得旋转矩阵R，平移向量t。其中，对极几何约束表达式如下：

p₂ ^TK^-Tt^RK^-1p₁＝0 (1)

E＝t^R (2)

在已经获得R和t的基础上，设定两帧图像中投影点的深度为s₁，s₂；其中深度s₁，s₂满足如下公式：

s₁x₁＝s₂Rx₂+t (3)

其中，x₁和x₂表示两个特征点R和t归一化后的坐标。最后通过最小二乘法进行求解出s₁和s₂。

所述S3中的初始位姿估计过程为采用稀疏图像对齐，最小化两个像素块之间的光度误差获取粗略的相机位姿，以及像素点之间的对应关系；具体包括如下过程：

首先设定参考空间点P和两个时刻的相机，参考空间点P在两个相机上成像，对应的投影点像素坐标分别为p₁，p₂；则空间点P在两帧图像中像素光度误差e为：

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

其中，I₁(p₁)表示中第一帧图像中像素坐标为p₁的像素光度；I₂(p₂)表示第二帧图像中像素坐标为p₂的像素光度；通过像素光度误差e获得优化一个空间点P的光度误差的二范数为：

min_ξJ(ξ)＝‖e‖² (4)

其中，ξ表示相机位姿的李代数，及相机位姿的一种表示形式；J(ξ)表示相机位姿的一个优化问题；

考虑空间中N个像素点的光度误差e_i＝I₁(p_1,i)-I₂(p_2,i),i∈(1,N)，则N个像素点的相机的位姿估计表示为：

这里优化变量是相机位姿的李代数表示形式ξ；I₁(p_1,i)表示中第一帧图像中第i个像素点的像素光度；I₂(p_2,i)表示中第二帧图像中第i个像素点的像素光度；e_i为第i个像素点的光度误差。

求式(5)中的优化问题，需要通过分析像素光度误差e和相机位姿的李代数ξ的导数关系。其中通过使用李代数的扰动模型，给exp(ξ)左乘一个设定的小扰动exp(δξ)获得：

其中，K为相机的内参；P为空间点P的世界坐标；Z₁表示空间点P的深度、Z₂表示空间点P在第二个相机坐标系下的深度；δξ通过设定的小扰动exp(δξ)获得；设定q为扰动分量在第二个相机坐标系的三维坐标，u为扰动分量的像素坐标，获得：

q＝δξ^exp(ξ^)P (7)

将式(7)、(8)代入式(6)，并对式(6)进行一阶泰勒公式展开，获得：

其中，

表示u处的像素梯度，

表示投影方程关于相机坐标系下的三维空间点的导数，

表示变换后的三维点对的导数；δξ通过设定的小扰动exp(δξ)获得；

最后推导出单个点误差相对于李代数ξ的雅克比矩阵；并通过列文伯格-马夸尔特方法进行增量计算，获得N个像素点的光度误差，迭代求解相机位姿J，表示为：

所述S4中，基于特征点法优化相机的初始位姿，包括如下步骤：

S41：首先需要将图像分成若干大小相等的网格，对每个网格进行Oriented FAST角点提取，使用四叉树分配算法均匀化特征点；

S42：构建直方图，通过角度一致性原理过滤特征点，获得候选匹配点；

S43：对保留下来的特征点进行BRIEF描述子的计算；描述子之间的距离表示两个特征点的相似程度；在本例中，首先找出所有描述子之间的最小距离和最大距离，当描述子之间的距离小于两倍最小距离时，则匹配成功，否则为误匹配；

S44：采用随机采样一致性RANSAC算法剔除掉大量误匹配的特征点；

S45：使用对极几何约束获得新的本质矩阵E2，并通过八点法对求得的本质矩阵E2奇异值分解，得到相机的位姿，包括旋转矩阵R2，平移向量t2。

所述S44中，由于特征点数据量比较大，而且特征信息匹配过程中容易出现误匹配，所以需要对误匹配的特征点进行剔除。

所述S5中，因为场景当中难免存在移动的物体，为了减少这部分的干扰所导致的误匹配，通过最小化地图点投影误差，来进一步优化相机的位姿。将三维地图点与当前帧的特征点进行匹配，利用最小化重投影误差优化当前位姿，具体包括如下过程：

首先设定空间点坐标P_i＝[X_i,Y_i,Z_i]^T，在当前帧中的像素坐标为u_i＝[u_i,v_i]^T,s_i为该点的深度，相机的位姿采用李代数的表示形式ξ。依据相机的针孔模型可得像素位置与空间点P的对应关系有：

转换为矩阵形式，表示为：

s_iu_i＝Kexp(ξ^)P_i (12)

在等式(12)存在一个误差，通过对误差求和，并构建最小误差函数，能够求解出最优的相机位姿ξ^*，表示为：

对于式(13)中的非线性优化问题，可以再使用高斯牛顿法或者列文伯格-马夸尔特方法进行求解，获得最优相机位姿ξ^*。

在实施过程中，通过直接法速度快以及特征点法精度、鲁棒性好的优势，将直接法与特征点法两种算法巧妙地结合，最后通过局部三维地图点的匹配来提高定位的准确性，保证在弱纹理环境下不会产生跟踪失败的情况；此外在光照变化、以及含有移动物体的场景中也能够通过特征点法保证定位的精度；另一方面通过采用单目相机，具有结构简单，易标定、操作方便，价格便宜等优点。

以上描述仅是本发明的一个具体实例，不构成对本发明的任何限制。显然对于本领域的专业人员来说，在了解了本发明内容和原理后，都可能在不背离本发明原理、结构的情况下，进行形式和细节上的各种修改和改变，但是这些基于本发明思想的修正和改变仍在本发明的权利要求保护范围之内。

Claims (8)

1.一种基于半直接法的单目视觉里程计的设计方法，其特征在于，包括如下步骤：

S1：通过相机采集图像数据，获得图像序列，并传输到分析设备上；其中分析设备为电脑，相机为单目相机；

S2：通过单目初始化流程，确定图像的尺度；

S3：基于直接法估计相机的初始位姿；

S4：基于特征点法优化相机的初始位姿；特征点法包括对图像进行ORB特征提取和特征匹配的过程；

S5：通过将三维地图点与当前帧的特征点进行匹配，利用最小化重投影误差进一步优化当前相机位姿；

S6：输出相机位姿，包括三维坐标点、平移向量和旋转向量；

S7：判断是否有新图像加入；若是则返回S3，进入算法的循环；否则结束流程。

2.根据权利要求1所述的一种基于半直接法的单目视觉里程计的设计方法，其特征在于，所述S2中的单目初始化流程包括如下步骤：

S21：移动相机，并获取图像，组成图像序列；其中，移动相机时，要求在设定方向上移动超过设定值的距离；

S22：选取图像序列中的前一帧作为初始帧，后一帧作为参考帧；

S23：通过对初始帧图像和参考帧图像进行ORB特征点提取和特征匹配，建立两帧图像之间的数据关联；

S24：利用三角测量得到初始帧中特征点的深度；其中三角测量表示通过两处观察同一个点的夹角，从而确定该点的距离。

3.根据权利要求1所述的一种基于半直接法的单目视觉里程计的设计方法，其特征在于，所述S23中的数据关联的建立，首先需要设定任意参考空间点P，空间点P在两帧图像中对应的投影点像素坐标分别为p₁，p₂；K为相机的内参，P为空间点P的世界坐标，设定初始帧到参考帧的相机运动变换为旋转矩阵R，平移向量t；然后通过对极几何约束求出本质矩阵E；最后使用八点法对求得的本质矩阵E进行奇异值分解，获得旋转矩阵R，平移向量t。

4.根据权利要求3所述的一种基于半直接法的单目视觉里程计的设计方法，其特征在于，所述对极几何表达式如下：

p₂ ^TK^-Tt^RK^-1p₁＝0 (1)

E＝t^R (2)

在已经获得R和t的基础上，设定两个特征点的深度为s₁，s₂；其中深度s₁，s₂满足如下公式：

s₁x₁＝s₂Rx₂+t (3)

其中，x₁和x₂表示两个特征点R和t归一化后的坐标；最后通过最小二乘法进行求解出s₁和s₂。

5.根据权利要求3所述的一种基于半直接法的单目视觉里程计的设计方法，其特征在于，所述S3中的初始位姿估计过程为采用稀疏图像对齐，最小化两个像素块之间的光度误差获取粗略的相机位姿，以及像素点之间的对应关系。

6.根据权利要求5所述的一种基于半直接法的单目视觉里程计的设计方法，其特征在于，所述初始位姿估计过程具体包括：

首先设定参考空间点P，参考空间点P在初始帧图像和参考帧图像中对应的投影点像素坐标分别为p₁，p₂；则空间点P在两帧图像中的像素光度误差e为：

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

其中，I₁(p₁)表示初始帧图像中像素坐标为p₁的像素光度；I₂(p₂)表示参考帧图像中像素坐标为p₂的像素光度；通过像素光度误差e获得优化一个空间点P的光度误差的二范数为：

min_ξJ(ξ)＝||e||² (4)

其中，ξ表示相机位姿的李代数；J(ξ)表示相机位姿；

考虑空间中N个像素点的光度误差e_i＝I₁(p_1，i)-I₂(p_2，i)，i∈(1，N)，则N个像素点的相机的位姿估计表示为：

求式(5)中的优化问题，需要通过分析像素光度误差e和相机位姿的李代数ξ的导数关系；其中通过使用李代数的扰动模型，给exp(ξ)左乘一个设定的小扰动exp(δξ)获得：

其中，K为相机的内参；P为空间点P的世界坐标；Z₁、Z₂分别表示；设定q为扰动分量在第二个相机坐标系的三维坐标，u为扰动分量的像素坐标，获得：

q＝δξ^exp(ξ^)P (7)

将式(7)、(8)代入式(6)，并对式(6)进行一阶泰勒公式展开，获得：

其中，

表示u处的像素梯度，

表示投影方程关于相机坐标系下的三维空间点的导数，

表示变换后的三维点对的导数；δξ通过设定的小扰动exp(δξ)获得；

最后推导出单个点误差相对于李代数ξ的雅克比矩阵；并通过列文伯格-马夸尔特方法进行增量计算，获得N个像素点的光度误差，迭代求解相机位姿J，表示为：

7.根据权利要求4所述的一种基于半直接法的单目视觉里程计的设计方法，其特征在于，所述S4中，基于特征点法优化相机的初始位姿，包括如下步骤：

S41：首先需要将图像分成若干大小相等的网格，对每个网格进行Oriented FAST角点提取，使用四叉树分配算法均匀化特征点；

S42：构建直方图，筛选出与主流方向一致的特征点作为候选匹配点；

S43：对保留下来的特征点进行BRIEF描述子的计算；其中，首先找出所有描述子之间的最小距离和最大距离，当描述子之间的距离小于两倍最小距离时，则匹配成功，否则为误匹配；描述子之间的距离表示两个特征点的相似程度；

S44：采用随机采样一致性RANSAC算法剔除掉大量误匹配的特征点；

S45：使用对极几何约束获得新的本质矩阵E2，并通过八点法对求得的本质矩阵E2进行奇异值分解，得到相机的位姿R2和t2；R2为旋转矩阵，t2为平移向量。

8.根据权利要求6所述的一种基于半直接法的单目视觉里程计的设计方法，其特征在于，所述S5中，将三维地图点与当前帧的特征点进行匹配，利用最小化重投影误差优化当前位姿，具体包括如下过程：

首先设定空间点坐标P_i＝[X_i，Y_i，Z_i]^T，在当前帧中的像素坐标为u_i＝[u_i，v_i]^T，s_i为该点的深度，相机的位姿采用李代数的表示形式ξ；依据相机的针孔模型可得像素位置与空间点P的对应关系有：

转换为矩阵形式，表示为：

s_iu_i＝Kexp(ξ^)P_i (12)

在等式(12)存在一个误差，通过对误差求和，并构建最小误差函数，能够求解出最优的相机位姿ξ^*，表示为：

对于式(13)中的非线性优化问题，可以再使用高斯牛顿法或者列文伯格-马夸尔特方法进行求解，获得最优相机位姿ξ^*。

Images