CN107564061B - 一种基于图像梯度联合优化的双目视觉里程计算方法 - Google Patents

Abstract

本发明公开了一种基于图像梯度联合优化的双目视觉里程计算方法。对输入双目视频进行预处理，计算双目视频帧的视差图获得度量距离；缩放形成多尺度的图像组成金字塔模型，计算特征获得一系列特征图像；根据度量距离用卡尔曼滤波器对图像进行处理预测估计相机姿态的运动过程，卡尔曼滤波器内搭建有相机的运动模型；使用基于梯度的双目视觉导航算法计算当前帧准确的相机姿态；用当前帧的相机姿态更新卡尔曼滤波器内的相机运动模型。本发明提出了结合两种梯度展开的优化算法，创新地利用图像梯度作为特征有效地避免了户外光照变化的影响，并且参考多个关键帧对相机姿态进行优化,得到了一个性能较好并可同时进行稠密三维重建的实时双目视觉里程计算方法。

Description

一种基于图像梯度联合优化的双目视觉里程计算方法

技术领域

本发明涉及计算机视觉领域以及图像处理的相关技术，尤其涉及了一种基于图像梯度联合优化的双目视觉里程计算方法。

背景技术

经过近些年的探索和发展，计算机视觉已经在机器人、自动驾驶、数字娱乐、医疗健康、安防监控、军事技术等很多领域具有应用场景。视觉里程计不仅是机器人与无人车视觉导航***的重要组成部分，也是计算机视觉和机器人领域的重要研究分支，本发明致力于提出一种快速有效的双目视觉里程计方法。视觉里程计是目标定位、姿态估计、地图绘制、场景理解等课题的研究基础和前提，一个优秀的视觉里程计算方法能为以上诸多研究课题提供强有力的支持和保障。

视觉里程计算方法的主要任务是从输入视频中实时估计出相机当前相对于起始参照状态的相对6自由度姿态，包括X-Y-Z三个坐标轴的相对位移，以及三个围绕坐标轴的旋转。同时，用于估计相机姿态的副产品为场景的稀疏或稠密重建。基于稀疏特征点匹配的方法需要三角化不同视频帧的匹配点得到三维稀疏重建，而基于直接图像对准的方法则利用双目视觉获得稠密的三维重建结果，为场景重建建立了基础。因此，视觉里程计算方法不仅提供了全局的姿态和位置信息，并且完成了初步的地图绘制与场景重建，是机器人以及无人车自主导航***的第一步，有着至关重要的作用。

视觉里程计是机器人和计算机视觉领域中的重要课题，是增强现实、自动驾驶汽车、机器人导航等应用的核心技术。目前主流的视觉里程计以及同时定位和地图重建算法通常采用了基于系数特征点匹配的解决方案，不能够完全利用整幅图像的信息，而且只能对场景进行稀疏重建。另一方面，基于直接图像对准的方案则使用整幅图像的信息，而且可以生成场景的密集点云，是具有广阔前景的技术方向。然而直接法的主要缺陷是相机小运动假设以及趋于收敛到局部最优，使该类方法在KITTI视觉里程计测试中性能弱于基于特征点匹配的策略。

在目前的视觉导航KITTI评测平台中，基于稠密观测的直接视觉导航方法未能取得理想的结果，其主要原因可以归结为下述几点：1)基于Lucas-Kanade框架[3]的图像对准理论收敛域有限，无法应对数据集中10Hz采样的大位移场景，需要非常好运动估计方法进行补偿；2)KITTI数据集源于实际的驾驶场景，涵盖包括城市、乡村和高速公路等不同的驾驶环境，直接使用像素灰度值与仿射光照模型无法应对如路面反光、树阴和建筑阴影等复杂环境光照环境影响；3)不完美的相机标定以及立体视觉匹配算法准确度直接影响视觉导航结果的精度，目前的立体视觉匹配均评估覆盖度内的视差估计精度，而视觉导航需要精确的点对点视差估计，位于立体视觉评测排行前列的算法未能有效提升导航精度。

发明内容

为了解决背景技术中存在的问题，本发明提出了一种基于图像梯度联合优化的双目视觉里程计算方法，实现了从视频序列中估计相机姿态和位置。

本发明采用的技术方案是包括如下步骤：

1)对输入双目视频进行预处理；

2)计算双目视频帧的视差图，获得度量距离；

3)将预处理后的图像进行缩放形成多个尺度的图像，组成金字塔模型，并对金字塔模型中的各个尺度图像计算特征，获得一系列特征图像；

4)根据度量距离用卡尔曼滤波器对图像进行处理预测估计相机姿态的运动过程，卡尔曼滤波器内搭建有相机的运动模型；

相机姿态包括相机的位置和朝向。

5)对图像金字塔模型中的各个尺度图像由粗尺度到精尺度的顺序使用基于梯度的双目视觉导航算法计算当前帧准确的相机姿态，并将粗尺度的输出作为精细尺度的输入；

粗尺度是指被缩放变小的图像，精尺度是指原输入图像。

6)用当前帧的相机姿态更新卡尔曼滤波器内的相机运动模型。

所述步骤6)是用当前帧的相机姿态更新卡尔曼滤波器内的相机运动模型中的模型参数。

所述步骤1)的预处理是进行高斯滤波和图像增强操作。

所述的图像增强具体是直方图均衡化增加对比度的处理。

所述步骤2)所述的视差图计算方法为线搜索匹配法，所述的度量距离为像素差的绝对值之和。

所述步骤3)中，所述的特征为图像梯度及其二阶梯度，用于所述步骤5)中计算图像匹配误差。

所述步骤4)使用卡尔曼滤波器预测获得当前帧视频图像中的相机姿态的运动过程，并用于所述步骤5)中来进一步计算准确的相机姿态。

所述步骤5)是按照由粗到精的顺序对相机姿态进行估计，通过双梯度优化方法解决稠密视觉导航的大位移难题，通过直接使用图像梯度作为图像特征获得了对光照变化的鲁棒性，同时也避免了增加仿射光照模型对***引起的额外计算代价，将多帧信息引入优化提升了***性能并减少漂移。

所述步骤5)双目视觉导航算法具体为：

5.1)构建用于计算当前帧变换T_c的以下联合直接双目视觉里程优化方程：

其中，ρ()为Tukey’s损失函数，π^-1表示为从图像空间转换到世界坐标系下的函数，π表示为从世界坐标系转换到图像空间的函数，T_c表示当前帧变换，T_p表示上一帧变换，T_k表示参考帧变换，Ω_k表示参考图像像素点的集合，Ω_p表示上一帧图像像素点的集合，x_i为第i个像素点的图像坐标位置，d_i为第i个像素点的深度估计值，θ为所求当前帧变换T_c的参数，▽为梯度计算符，I_p为上一帧图像，I_k为参考帧图像，I_c为当前帧图像；

所述参考帧是指当前帧之前的第m帧，在本发明实施中m＝12，在前12帧时，参考帧为第一帧，

例如，对于初始时刻的第一帧，第一帧姿态置零，朝向和位置均置为零；

对于第二到m帧，均以第一帧作为参考帧，

对于第i帧，以第i-m帧作为参考帧。

所述的Tukey’s损失函数ρ()采用以下公式计算如下：

其中，κ为Tukey’s损失函数参数，在发明实例中为4.6851，t表示Tukey’s损失函数的输入；

5.2)上述方程是一个非线性优化问题，采用以下公式的高斯-牛顿方法对联合直接双目视觉里程优化方程进行迭代优化来计算当前帧变换T_c，再用当前帧变换T_c对当前帧图像对应的姿态进行变换，获得当前帧准确的相机姿态。

由此依次对每一帧按时间顺序进行处理，可以获得每一帧准确的相机姿态。

所述第i个像素点的深度估计值d_i是通过双目视觉深度估计算方法(StereoDepth Estimation)估计计算获得。

所述步骤5.2)具体为：

5.2.1)采用以下公式计算获得所求当前帧变换T_c的参数θ：

θ＝-(J^TW_J)^-1J^TWr.

其中，J是Jacobian矩阵，J^T表示Jacobian矩阵的转置，r代表残差，

W为对角矩阵，对角矩阵W中每个对角线位置上为Tukey’s损失函数ρ的导数，

5.2.2)根据李代数定义采用以下公式计算获得临时变换T(θ)：

其中，ν为位移，[ω]_×为反对称矩阵(Skew-symmetric matrix)，[ω]_×＝ω₁G₁+ω₂G₂+ω₃G₃，ω₁、ω₂、ω₃分别表示三轴坐标下的三个旋转角度，G1，G2，G3为反对称矩阵的基，分别形式如下：

上述中，李代数定义是指是数学上的代数结构，主要用于研究象李群和微分流形之类的几何对象。

5.2.3)根据李代数定义，根据临时变换T(θ)采用以下公式计算变换，并赋值到当前帧变换T_c：

T_c←T_cT(θ)

其中，T(θ)表示临时变换；

5.2.4)再以当前帧变换T_c重新构建对应的Jacobian矩阵，以重复上述步骤以金字塔模型进行迭代计算，以卡尔曼滤波器的预测输出作为初始第一层的输入，以上一层的输出作为下一层的输入，直到满足迭代计算的停止条件，停止条件为达到|θ|＜0.001或达到最大迭代数(25×(l+1))，其中l为所在金字塔模型层数。

对于初始帧计算时，初始帧变换T_c均为0，姿态为0，并构建获得初始帧变换T_c对应的Jacobian矩阵。

所述Jacobian矩阵是针对每一帧图像而设的，Jacobian矩阵大小和图像大小相同。

所述步骤5.2.1)中的Jacobian矩阵采用以下方式构建获得：

针对除最后一层以外的其他每一层均采用以下公式计算得到每个像素点在Jacobian矩阵中的元素值，组成所述步骤5.2.1)中的Jacobian矩阵：

其中，x_i,y_i,z_i为第i个像素点在三维世界中坐标值，f_u,f_v为相机焦距参数，J_i(θ)表示图像第i个像素点对应到Jacobian矩阵中的元素值，i表示像素点的序号，I_esm(x_i)表示当前帧的有效二阶最小化(efficient second order minimization)；

针对最后一层采用以下公式计算得到每个像素点在Jacobian矩阵中的元素值，组成所述步骤5.2.1)中的Jacobian矩阵：

其中，

为相对像主点图像坐标值，

(u_i,v_i)为第i个像素的图像坐标值，(c_u，c_v)为照相机的像主点(principal point)；

当前帧的有效二阶最小化I_esm(x_i)采用以下公式计算：

其中，I_c为当前帧图像，I_k为参考帧。

所述第i个像素点在三维世界中坐标值x_i,y_i,z_i是采用以下方式获得：

针孔相机的投影方程定义为π()，通过双目视觉深度估计算方法(Stereo DepthEstimation)估计计算深度d，用深度d通过逆投影方程获得三维点坐标X＝(x_i,y_i,z_i)：

其中，π^-1为从图像空间转换到世界坐标系下的函数，x_i为第i个像素点的图像坐标位置，d_i为深度估计算方法所得深度估计值。

本发明中所定义刚性变换函数T(包括T_c、T_p和T_k)在SE(3)中的变换，SE(3)表示三维Lie群，这样视觉导航的目标为从图像观测中估计该帧图像的变换函数T＝(R,m)∈SE(3)，其中R是相机的方向矩阵，R∈SO(3)，SO(3)表示所有环绕着三维欧几里得空间的原点的旋转，组成的群，m是位置向量，m∈R³。

本发明方法在具体实施采用包括以下三个模块的***：

1)输入模块，用于接收采集到的双目视频序列；

2)视觉里程计模块，用于分析图像，估计出相机当前的姿态和位置信息；

3)输出模块，将相机姿态和三维重建结果绘制出来。

传统稠密视觉导航算法建立在准确的相机内参数校准和深度观测值假设基础上，难以应对实际***中相机校准误差与存在大量噪声的视差估计。根据泰勒展开理论，目标值附近的梯度估计相对会准确很多，不同位置的泰勒展开会有不同的梯度估计。

而本发明提出的算法，在粗尺度时采用收敛域大且精度低的梯度进行高斯-牛顿迭代，在细尺度使用收敛域小高精度的梯度。在金子塔低精度图像使用上述J进行计算，而在最后一层高精度层中采用了独创性的

进行计算。由此，本发明可以得到更稳定的梯度关系，从而更稳定地计算所求优化问题的结果。

本发明的有益效果是：

本发明实现了实时双目导航的同时重构三维场景，通过双梯度优化方法解决稠密视觉导航的大位移难题；通过直接使用图像梯度作为图像特征获得了对光照变化的鲁棒性，同时也避免了增加仿射光照模型对***引起的额外计算代价；将多帧信息引入优化提升了***性能并减少漂移。

本发明提出了结合两种梯度展开的优化算法，创新地利用图像梯度作为特征有效地避免了户外光照变化的影响，并且参考多个关键帧对相机姿态进行优化,得到了一个性能较好并可同时进行稠密三维重建的实时双目视觉里程计算方法。

附图说明

图1为本发明算法的整体结构示意图。

图2为本发明方法的流程图。

图3为实施例在KITTI训练集中的相机轨迹结果图。

图4为实施例估计的三维点云与相机跟踪轨迹图。

表1为KITTI评测中的各类方法对比。

具体实施方式

下面将结合本发明中的附图，对本发明的技术方案进行清晰、完整地描述。基于本发明中的实施例，本领域普通技术人员在没有做创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的目的、技术方案及优点更加清楚明白，接下来将参照附图对本发明实施例进行详细的说明。

如图1所示，本发明实施例具体实施的***模块为：

输入模块，用来接收用户提供的双目视频序列，接下来送入视觉里程计模块进行相机姿态估计。

视觉里程计模块，把接收到的视频帧进行预处理，然后构建图像特征金字塔、使用双目视觉里程计算方法进行估计等一系列操作，估计出视频中的相机所在的位置及其姿态信息，并送入输出模块。

输出模块，用于显示相机轨迹和三维重建结果，把相机轨迹和姿态在三维重建中的相应位置上呈现出来。

为验证本发明，在本领域已公开的具有挑战的车载视觉导航数据集KITTI上进行设计实验，将本发明算法与公开的一系列算法进行对比，把实验结果与测试集的高精度惯性导航仪标注进行对比，绘制出相机轨迹和统计相对位移与旋转误差作为衡量算法性能的标准。

如图3所示，本发明提出的算法可以精确恢复相机轨迹。图中可见采用本发明恢复的相机轨迹非常接近超高精度惯性测量单元的测量值。

如图4所示，本发明可以稠密重建三维场景。图中可见本发明不仅恢复了稠密的三维重建，而且在三维场景中准确标出了6自由度相机姿态。

KITTI视觉导航测试平台测试集包括11段不同驾驶场景的视频以及高精度惯性导航标注，而且标注信息未公开，需由测试方反馈测试结果，本发明算法比公开的同类方法获得了更低的位移与旋转误差，并且甚至优于激光雷达的解决方案，如表1所示。

表1:KITTI评测中的各类方法对比(2017年3月)

上表中可见本发明的结果在公开测试中不仅性能优于传统方法，并且可以恢复出场景稠密的三维结构。

由此可见本发明算法均取得视觉导航领域中常用的算法更优秀的实验结果。

最后，应当指出，以上实施例仅是本发明较有代表性的例子。显然，本发明的技术方案并不限于上述实施例，还可以有许多变形。本领域的普通技术人员可在不脱离本发明的发明思想情况下，对于上述实施例做出种种修改或变化，因而本发明的保护范围并不被上述实施例所限，而应该是符合权利要求书提到的创新性特征的最大范围。

Claims (9)

1.一种基于图像梯度联合优化的双目视觉里程计算方法，其特征在于包括如下步骤：

1)对输入双目视频进行预处理；

2)计算双目视频帧的视差图，获得度量距离；

3)将预处理后的图像进行缩放形成多个尺度的图像，组成金字塔模型，并对金字塔模型中的各个尺度图像计算特征，获得一系列特征图像；

4)根据度量距离用卡尔曼滤波器对图像进行处理预测估计相机姿态的运动过程，卡尔曼滤波器内搭建有相机的运动模型；

5)对图像金字塔模型中的各个尺度图像使用基于梯度的双目视觉导航算法计算当前帧准确的相机姿态；

所述步骤5)双目视觉导航算法具体为：

5.1)构建用于计算当前帧变换T_c的以下联合直接双目视觉里程优化方程：

其中，ρ()为Tukey’s损失函数，π^-1表示为从图像空间转换到世界坐标系下的函数，π表示为从世界坐标系转换到图像空间的函数，T_c表示当前帧变换，T_p表示上一帧变换，T_k表示参考帧变换，Ω_k表示参考图像像素点的集合，Ω_p表示上一帧图像像素点的集合，x_i为第i个像素点的图像坐标位置，d_i为第i个像素点的深度估计值，θ为所求当前帧变换T_c的参数，▽为梯度计算符，I_p为上一帧图像，I_k为参考帧图像，I_c为当前帧图像；

所述的Tukey’s损失函数ρ()采用以下公式计算如下：

其中，κ为Tukey’s损失函数参数，t表示Tukey’s损失函数的输入；

5.2)高斯-牛顿方法对联合直接双目视觉里程优化方程进行迭代优化来计算当前帧变换T_c，再用当前帧变换T_c对当前帧图像对应的姿态进行变换，获得当前帧准确的相机姿态；

6)用当前帧的相机姿态更新卡尔曼滤波器内的相机运动模型。

2.根据权利要求1所述的一种基于图像梯度联合优化的双目视觉里程计算方法，其特征在于：所述步骤1)的预处理是进行高斯滤波和图像增强操作。

3.根据权利要求1所述的一种基于图像梯度联合优化的双目视觉里程计算方法，其特征在于：所述步骤2)所述的视差图计算方法为线搜索匹配法，所述的度量距离为像素差的绝对值之和。

4.根据权利要求1所述的一种基于图像梯度联合优化的双目视觉里程计算方法，其特征在于：所述步骤3)中，所述的特征为图像梯度及其二阶梯度。

5.根据权利要求1所述的一种基于图像梯度联合优化的双目视觉里程计算方法，其特征在于：所述步骤4)使用卡尔曼滤波器预测获得当前帧视频图像中的相机姿态的运动过程。

6.根据权利要求1所述的一种基于图像梯度联合优化的双目视觉里程计算方法，其特征在于：所述第i个像素点的深度估计值d_i是通过双目视觉深度估计算方法(Stereo DepthEstimation)估计计算获得。

7.根据权利要求1所述的一种基于图像梯度联合优化的双目视觉里程计算方法，其特征在于：所述步骤5.2)具体为：

5.2.1)采用以下公式计算获得所求当前帧变换T_c的参数θ：

θ＝-(J^TWJ)^-1J^TWr.

其中，J是Jacobian矩阵，J^T表示Jacobian矩阵的转置，r代表残差，

W为对角矩阵，对角矩阵W中每个对角线位置上为Tukey’s损失函数ρ的导数，

5.2.2)根据李代数定义采用以下公式计算获得临时变换T(θ)：

其中，v为位移，[ω]_×为反对称矩阵(Skew-symmetric matrix)，[ω]_×＝ω₁G₁+ω₂G₂+ω₃G₃，ω₁、ω₂、ω₃分别表示三轴坐标下的三个旋转角度，G1，G2，G3为反对称矩阵的基，分别形式如下：

5.2.3)根据李代数定义，根据临时变换T(θ)采用以下公式计算变换，并赋值到当前帧变换T_c：

T_c←T_cT(θ)

其中，T(θ)表示临时变换；

5.2.4)再以当前帧变换T_c重新构建对应的Jacobian矩阵，以重复上述步骤以金字塔模型进行迭代计算，以卡尔曼滤波器的预测输出作为初始第一层的输入，以上一层的输出作为下一层的输入，直到满足迭代计算的停止条件，停止条件为达到|θ|＜0.001或达到最大迭代数(25×(l+1))，其中l为所在金字塔模型层数。

8.根据权利要求7所述的一种基于图像梯度联合优化的双目视觉里程计算方法，其特征在于：所述步骤5.2.1)中的Jacobian矩阵采用以下方式构建获得：

针对除最后一层以外的其他每一层均采用以下公式计算得到每个像素点在Jacobian矩阵中的元素值，组成所述步骤5.2.1)中的Jacobian矩阵：

其中，x_i,y_i,z_i为第i个像素点在三维世界中坐标值，f_u,f_v为相机焦距参数，J_i(θ)表示图像第i个像素点对应到Jacobian矩阵中的元素值，i表示像素点的序号，I_esm(x_i)表示当前帧的有效二阶最小化(efficient second order minimization)；

针对最后一层采用以下公式计算得到每个像素点在Jacobian矩阵中的元素值，组成所述步骤5.2.1)中的Jacobian矩阵：

其中，

为相对像主点图像坐标值，

(u_i,v_i)为第i个像素的图像坐标值，(c_u，c_v)为照相机的像主点(principal point)；

当前帧的有效二阶最小化I_esm(x_i)采用以下公式计算：

其中，I_c为当前帧图像，I_k为参考帧图像。

9.根据权利要求8所述的一种基于图像梯度联合优化的双目视觉里程计算方法，其特征在于：所述第i个像素点在三维世界中坐标值x_i,y_i,z_i是采用以下方式获得：

针孔相机的投影方程定义为π()，通过双目视觉深度估计算方法(Stereo DepthEstimation)估计计算深度d，用深度d通过逆投影方程获得三维点坐标X＝(x_i,y_i,z_i)：

其中，π^-1为从图像空间转换到世界坐标系下的函数，x_i为第i个像素点的图像坐标位置，d_i为深度估计算方法所得深度估计值。

Images