CN111932616B - 一种利用并行计算加速的双目视觉惯性里程计方法 - Google Patents

Abstract

本发明提出一种利用并行计算加速的双目视觉惯性里程计方法，用于低性能处理器上的实时位置和姿态估计，属于定位技术领域。本发明基于VINS‑Fusion算法，对其视觉前端算法进行了改进，主要在于：多次光流跟踪的计算冗余去除、角点检测和光流跟踪的并行执行设计和基于FPGA的角点检测算法实现。本发明是一套基于优化的视觉惯性里程计方法，有效地缩短了算法的运行耗时，可用于低性能处理器上的实时位姿估计，定位精度高，鲁棒性好，具有良好的工程应用和推广价值。

Description

一种利用并行计算加速的双目视觉惯性里程计方法

技术领域

本发明涉及定位技术领域，具体涉及一种利用并行计算加速的双目视觉惯性里程计方法。

背景技术

视觉惯性里程计是一种基于单/双目相机和惯性测量单元(IMU)的自主定位技术。这项技术通过融合单/双目相机提供的视觉信息和IMU提供的设备运动信息，可以对设备的位姿做出准确快速的估计。该技术致力于解决在无法使用外部定位技术(如GPS和UWB定位技术)的情况下，处于未知环境中的无人移动平台对自身位姿的估计问题。在自动驾驶、无人机和增强现实等领域，这项技术已经催生了许多商业应用，比如苹果公司的增强现实解决方案ARkit和大疆公司的Mavic AIR无人机都采用了该技术进行位姿估计。

相比于其他无需依赖外部定位设备的定位方案，视觉惯性里程计在传感器的选择上具有独特的优势。与基于激光雷达的定位方案相比，IMU和相机的搭配方案成本低、重量轻，易于部署在小型移动平台上。与基于单/双目相机的定位方案相比，添加的IMU可以提供短时间内的准确估计，能够适应快速运动，弥补纯视觉的不足，还能提供尺度信息。

尽管在普通的个人计算机上，视觉惯性里程计算法已经有了很好的精度和速度表现，但由于其过大的计算负担，目前还很难在低性能处理器(比如ARM Cortex-A9)上做到实时处理。这限制了该算法在资源受限场景(比如手机和微型无人机)的应用。这些场景通常会附带有尺寸、重量、成本和功耗方面的要求，使得高性能的处理器无法使用。因此，对于运行在低性能处理器上的视觉惯性里程计算法，必须进行改进，以提升速度，改善算法的实时性。

现有的视觉惯性里程计算法一般包含前端和后端两部分。其中，前端的图像处理部分和后端的位姿估计部分是整套算法中计算量最大的部分，也是加快算法速度的关键。2012年，美国加州大学的Li Mingyang等人设计了一套融合EKF-SLAM(基于扩展卡尔曼滤波的同时定位与建图算法)和MSCKF(多状态约束下的卡尔曼滤波算法)的视觉惯性里程计算法，通过动态调整特征点数目和滑动窗口长度，对后端算法的耗时加以控制。但是，该方案没有对耗时较大的图像处理部分进行加速，也没有采用定位精度更高的优化方法进行位姿估计。2013年，德国Roboception GmbH公司的研究员Korbinian Schmid等人提出了一种基于双目立体匹配的视觉惯性里程计算法，其中，采用FPGA(可编程逻辑块)实现了SGM算法(半全局匹配算法)，可从双目图像快速恢复场景深度，但图像处理部分中的Harris角点检测算法仍在CPU上执行，没有获得速度提升。

发明内容

本发明的目的是为克服已有技术的不足之处，提出一种利用并行计算加速的双目视觉惯性里程计方法。本发明是一套基于优化的视觉惯性里程计方法，通过光流跟踪和角点检测的并行化、角点检测的FPGA加速和冗余计算的剔除，有效地缩短了算法的运行耗时，可用于低性能处理器上的实时位姿估计，具有良好的工程应用和推广价值。

本发明一种利用并行计算加速的双目视觉惯性里程计方法，该方法所有步骤被分配到两个线程中执行；其中，步骤1)到步骤9)在线程1中执行，步骤10)到步骤21)在线程2中执行；从第2帧图像开始，两个线程并行执行；在线程1中，在可编程逻辑块FPGA上执行的步骤2)与在CPU执行的步骤3)到步骤4)并行执行；其特征在于，该方法包括以下步骤：

1)从惯性测量单元IMU读取IMU数据，从双目相机读取双目图像数据并作为线程1的当前帧，其中每帧双目图像包含一张左目图像和一张右目图像；将该当前帧的左目图像发送给FPGA；

当该当前帧为第1帧时，设置一个初始为空的特征点列表并作为当前特征点列表，令特征点列表最大长度为N，N为正整数；

2)在FPGA上，利用角点检测方法，对线程1的当前帧的左目图像进行Harris角点提取，得到线程1当前帧左目图像的角点坐标数组；具体步骤如下：

2-1)设置一个角点坐标数组，初始为空，数组长度为N；设置角点判别阈值；设置一个角点数目寄存器，初始值为0，最大值为N；

2-2)在FPGA中，依次提取线程1当前帧的左目图像的像素并作为当前像素；对每个当前像素，将该左目图像与两个方向的Sobel算子进行卷积，求出当前像素处的图像梯度[g_x，g_y]；其中，g_x为当前像素处沿行方向的图像梯度，g_y为当前像素处沿行方向的图像梯度，沿行方向的Sobel算子为

沿列方向的Sobel算子为

2-3)根据步骤2-2)得到的当前像素处的图像梯度[g_x，g_y]，计算当前像素处的自相关矩阵

其中，自相关矩阵的三个分量A、B和C，计算表达式如式(1)至式(3)所示：

A＝g_x ² (1)

B＝g_xg_y (2)

C＝g_y ² (3)

2-4)对步骤2-3)得到的自相关矩阵三个分量A、B和C分别进行均值滤波，得到滤波后的自相关矩阵分量A′、B′和C′；

2-5)利用步骤2-4)得到的A′、B′和C′，计算当前像素处的角点响应值R，表达式如下：

R＝A′*C′-B^′2-0.04*(A′+C′)² (4)

2-6)依据步骤2-5)求出的角点响应值，判断当前像素是否为角点并得到相应的角点判别值；

角点判别条件包括两条：

2-6-1)判定当前像素的角点响应值是否高于步骤2-1)中设置的角点判别阈值；2-6-2)判定当前像素的角点响应值是否高于邻近像素的角点响应值，其中邻近像素是指以当前像素为中心的大小为15×15的矩形区域内除当前像素外的所有像素；

若当前像素的角点响应值同时满足两条角点判别条件，则标记当前像素的角点判别值为1；若任一条件不满足，则标记当前像素的角点判别值为0；

2-7)若步骤2-6)得到的当前像素的角点判别值为1且角点数目寄存器的值小于500，则将当前像素的行列坐标记录到角点坐标数组中，更新角点坐标数组，同时角点数目寄存器的值加1；若步骤2-6)得到的当前像素的角点判别值为0或角点数目寄存器的值不小于500，则不进行任何操作；

对当前帧的左目图像中的所有像素的角点判别值判定完毕后，线程1当前帧左目图像的角点坐标数组更新完毕；

3)对线程1的当前帧进行判定：若该当前帧是第1帧，则直接进入步骤5)；若该当前帧不是第1帧，则进入步骤4)；

4)在线程1的前一帧左目图像和当前帧左目图像间进行光流跟踪，计算当前特征点列表中的特征点在当前帧左目图像中的坐标，并得到线程1当前帧最终跟踪成功的特征点集合；具体步骤如下：

4-1)对线程1当前帧左目图像进行高斯滤波和降采样，生成该当前帧左目图像的图像金字塔，该图像金字塔层数为三层；

4-2)将步骤4-1)的图像金字塔中的每层图像分别与Scharr算子进行卷积，计算每层图像对应的图像梯度图；

4-3)根据线程1前一帧左目图像的图像金字塔和图像梯度图、当前帧左目图像的图像金字塔和图像梯度图，使用Lucas-Kanade光流法，对当前特征点列表中的特征点进行跟踪，得到当前帧左目图像初步跟踪成功的特征点集合；

4-4)利用步骤4-3)得到的当前帧左目图像初步跟踪成功的特征点集合，使用Lucas-Kanade光流法，依据该集合中每个特征点在当前帧左目图像中的坐标，计算该特征点在前一帧左目图像中的坐标；

若计算得到的坐标和该特征点的原始坐标的距离大于1个像素，则该特征点跟踪失败，将该特征点从当前帧左目图像初步跟踪成功的特征点集合中删除；

对步骤4-3)得到的集合中的所有特征点处理完毕后，得到线程1当前帧左目图像最终跟踪成功的特征点集合，该集合包含了当前特征点列表中每个跟踪成功的特征点在当前帧左目图像中的坐标；

5)依据步骤2)得到的当前帧左目图像的角点坐标数组和步骤4)得到的线程1当前帧最终跟踪成功的特征点集合，更新当前特征点列表中的左目坐标；具体步骤如下：

5-1)生成一张与线程1当前帧左目图像相同大小的二值图像，并将该二值图像的每个像素的初始值设置为0；

5-2)判定：如果线程1的当前帧没有进行步骤4)，则进入步骤5-5)，否则，进入步骤5-3)；

5-3)从步骤4)得到的线程1当前帧最终跟踪成功的特征点集合中依次取出每个特征点坐标，该坐标为该特征点在线程1当前帧左目图像中的坐标；每次取出一个特征点的坐标后，查询二值图像中该特征点坐标对应位置的像素值是否为0；若二值图像中该对应位置的像素值为0，则将当前特征点列表中该特征点对应的左目坐标更新为该特征点坐标，并以该特征点坐标在二值图像中的对应位置为圆心，30个像素为半径，绘制圆，将圆内的所有二值图像像素值更新为1；若二值图像中该对应位置的像素值为1，则不进行任何操作；

5-4)删除当前特征点列表中所有未在步骤5-3)中更新左目坐标的特征点，更新当前特征点列表；

5-5)从步骤2)得到的当前帧左目图像的角点坐标数组中依次取出每个角点坐标；每次取出一个角点坐标后，查询二值图像中该角点坐标对应位置的像素值是否为0；若该对应位置的像素值为0且当前特征点列表的长度低于设定的最大长度，则将该角点坐标作为一个新的特征点的左目坐标并加入到当前特征点列表中，更新当前特征点列表，然后以该角点坐标在二值图像中的对应位置为圆心，30个像素为半径，绘制圆，将圆内的所有二值图像像素值更新为1；若二值图像中该对应位置的像素值为1，则不进行任何操作；

对当前帧左目图像的角点坐标数组中所有角点坐标处理完毕后，得到更新左目坐标后的当前特征点列表，其中该更新后的当前特征点列表中的特征点为当前帧左目图像的特征点；

6)在线程1当前帧左目图像和线程1当前帧右目图像间进行光流跟踪，获得该当前帧左目图像的特征点在该当前帧右目图像中的坐标，得到初始更新完毕的当前特征点列表；具体步骤如下：

6-1)对线程1当前帧右目图像进行高斯滤波和降采样，生成该当前帧右目图像的图像金字塔，该图像金字塔层数为三层；

6-2)将步骤6-1)得到的图像金字塔中的每层图像与Scharr算子进行卷积运算，计算每层图像对应的图像梯度图；

6-3)根据线程1当前帧左目图像的图像金字塔和图像梯度图、当前帧右目图像的图像金字塔和图像梯度图，使用Lucas-Kanade光流法，对步骤5)更新后的当前特征点列表中的特征点进行跟踪，得到当前帧右目图像初步跟踪成功的特征点集合；

6-4)对步骤6-3)中得到的当前帧右目图像初步跟踪成功的特征点集合，使用Lucas-Kanade光流法，依据该集合中的每个特征点在当前帧右目图像中的坐标，计算该特征点在当前帧左目图像中的坐标；

若计算得到的坐标和该特征点的原始左目坐标的距离大于1个像素，则该特征点跟踪失败，将该特征点从当前帧右目图像初步跟踪成功的特征点集合中删除；

对步骤6-3)得到的集合中的所有特征点处理完毕后，得到线程1当前帧右目图像最终的跟踪成功的特征点集合，该集合包含了当前特征点列表中每个最终跟踪成功的特征点在当前帧右目图像中的坐标；

6-5)对步骤6-4)得到的线程1当前帧右目图像最终跟踪成功的特征点集合中的每个特征点，将当前特征点列表中该特征点对应的右目坐标修改为步骤6-3)得到的该特征点在当前帧右目图像中的坐标；将当前特征点列表中的其余特征点的右目坐标标记为未获取；修改完毕后，得到初始更新完毕的当前特征点列表；

7)对步骤6)得到的初始更新完毕的当前特征点列表中每个特征点的左右目坐标进行畸变矫正，更新该特征点列表中每个特征点的左右目坐标，得到最终更新完毕的当前特征点列表；

8)对线程1的当前帧判定：若该当前帧是第1+3i帧，其中，i为自然数，i＝0,1,2…，则将该当前帧对应的步骤7)更新完毕的当前特征点列表发送给线程2，进入步骤9)；否则，不进行任何操作，进入步骤9)；

9)对线程1的当前帧判定：若该当前帧是第4+3i帧，其中i为自然数，i＝0,1,2…，则将第1+3i帧和第4+3i帧间的所有IMU数据发送给线程2，然后当双目相机获取新一帧图像时，重新返回步骤1)；否则，不进行任何操作，然后当双目相机获取新一帧图像时，重新返回步骤1)；

10)从线程1获取IMU数据和当前特征点列表，将所获取的当前特征点列表对应的双目图像作为线程2的当前帧；

当线程2的当前帧为第1帧时，设置大小为10帧的滑动窗口，用以保存线程2中每帧图像对应的当前特征点列表和任意相邻两帧间的IMU预积分结果；

11)对当前滑动窗口内帧数进行判定：若滑动窗口内的帧数少于10帧，则转到步骤12)，进行双目视觉惯性里程计的初始化；否则，转到步骤17)，采用非线性优化算法，对滑动窗口内各帧对应的相机位姿进行估计；

12)将线程2的当前帧放入滑动窗口中，作为滑动窗口内的最新帧；对于该当前帧对应的特征点列表中同时具有左目坐标和右目坐标的特征点进行三角化，求出该特征点的深度；

13)利用步骤12)的结果，使用PnP算法，计算线程2的当前帧对应的相机位姿；

14)若滑动窗口内的帧数大于1，则对滑动窗口中最新两帧间的IMU数据进行预积分，并将该预积分结果存储于滑动窗口中，然后进入步骤15)；否则，不进行任何操作，直接进入步骤15)；

15)若滑动窗口内的帧数少于10帧，则重新返回步骤10)，从线程1获取新的IMU数据和特征点列表；否则，转到步骤16)；

16)将滑动窗口内各帧的相机位姿与IMU预积分值对齐，求解IMU中的陀螺仪的偏置初始值；

17)计算滑动窗口内的最新帧左目图像相对滑动窗口内的次新帧左目图像的平均视差量；

平均视差量的计算方法是遍历两张左目图像共有的所有特征点，对共有的所有特征点在两幅图像的坐标变化量进行累加，最后除以共有的特征点的总个数，作为这两张图像的平均视差量；

18)当步骤17)得到的平均视差量高于10个像素时，则标记滑动窗口内的最新帧为关键帧；否则，标记滑动窗口内的最新帧为非关键帧；

19)取出滑动窗口内的最新帧和线程2的当前帧间的所有IMU数据，进行IMU预积分；

20)构建与线程2的当前帧关联的视觉误差项和IMU误差项，添加入目标函数L；其中，目标函数中的优化变量X包含了滑动窗口内所有帧的设备状态和所有被滑动窗口内多于1帧观测的特征点的逆深度，其中被滑动窗口内多于1帧观测的特征点的数目记为m；

将滑动窗口内第k帧的设备状态记为x_k，k＝1，...10，将线程2的当前帧的序号记为k＝11，则线程2的当前帧的设备状态记为x₁₁；将世界坐标系记为w系，第k帧时的IMU坐标系记为b_k系；每一帧的设备状态包含了该帧时IMU相对世界坐标系的位置

速度

旋转

加速度计偏置

和陀螺仪偏置

第j个特征点的逆深度记为λ_j，j＝1，...m；则优化变量X的表达式如下：

X＝[x₁，x₁，…x₁₁，λ₀，λ₁，…λ_m] (5)

目标函数L定义为：

其中，式(7)中等式右边第一项为边缘化产生的先验信息项，第二项为IMU误差项，第三项是视觉误差项；z_IMU，k是第k帧到第k+1帧间的IMU预积分值，P_k是IMU误差项的协方差矩阵；C_k是第k帧所观测到的所有特征点的编号集合，z_CAM，k，j是通过光流跟踪确定的第j个特征点在第k帧左目和右目图像上的坐标，P_C是视觉误差项的协方差矩阵，按照下式计算，f代表相机焦距：

将IMU预积分值和帧间位姿变化量求差，得到IMU误差项，计算表达式如式(9)所示：

其中，

代表从世界坐标系到第k帧的IMU坐标系的旋转矩阵，g^w为重力向量在世界坐标系下的表达，Δt_k为第k帧和第k+1帧的时间间隔；

和

为从第k帧到第k+1帧的IMU预积分值；[·]_xyz代表取四元数的虚数分量，

代表四元数乘法；

通过求取m个特征点在左目图像的投影位置和光流跟踪给出的实际位置的差距，得到视觉误差项；

对于同时具有左目坐标和右目坐标的特征点，先将该特征点坐标从右目坐标系转换到左目坐标系，然后采用式(10)计算该特征点的视觉误差项：

其中，

是通过光流跟踪确定的编号为j的特征点在第k帧的相机归一化坐标系中的坐标，

是编号为j的特征点在第k帧的相机坐标系中的坐标，||·||是取模运算；取一个与

相垂直的平面，在该平面任意选取一组单位正交基，记作

b_x和b_y分别是

在第k帧的相机坐标系中的表达；

采用高斯-牛顿法最小化目标函数L，获得优化变量X的增量，并对优化变量X进行更新；

21)对更新后的优化变量X进行边缘化；

若滑动窗口内的最新帧为关键帧，则边缘化滑动窗口内的最老帧所对应的设备状态变量和最老帧所观测的特征点对应的逆深度变量，并将这两种变量的估计值转化为先验信息，加入到目标函数L中，然后将线程2的当前帧作为最新帧加入滑动窗口，滑动窗口更新完毕，然后重新返回步骤10)；

若滑动窗口内的最新帧不是关键帧，则舍弃滑动窗口内的最新帧和该帧的对应误差项，但保留滑动窗口内的最新帧和次新帧间的IMU预积分结果，将线程2的当前帧作为最新帧加入滑动窗口，滑动窗口更新完毕，然后重新返回步骤10)。本发明的特点及有益效果在于：

本发明是一套基于优化的双目视觉惯性里程计方法，定位精度高，鲁棒性好。通过光流跟踪和角点检测的并行化、角点检测的FPGA加速和冗余计算的剔除，加快了双目视觉惯性里程计算法的运行速度，将可接受的最大帧率提升至原来的三倍，有效解决了低性能处理器上的算法实时性问题。本发明主要用于移动设备的实时位姿估计，可为增强现实、无人机和自动驾驶等上层应用提供准确和实时的位置和姿态估计。相比其他定位技术，比如GPS定位技术、UWB定位技术和基于激光雷达的定位技术等，本发明具有三点优势：无需依赖任何外部定位设备，使用场景不受限制；不需要环境的先验地图；只需要廉价IMU和双目相机，成本低，重量轻，速度快，无需高性能处理器即可做到实时处理。以上优势使得本发明的适用范围很广，包括各类室内外场景和对功耗、重量敏感的移动设备(比如小型无人机和手机)。

附图说明

图1是本发明实施例的整体硬件结构示意图。

图2是本发明实施例对某条测试序列的输出轨迹相对真值的偏差图。

图3是本发明方法的整体流程图。

图4是本发明方法中的图像处理部分的流程图。

图5是本发明实施例中左目图像与Sobel算子进行卷积的结构示意图。

图6是本发明实施例中自相关矩阵计算的结构示意图。

图7是本发明实施例中对自相关矩阵分量进行均值滤波的结构示意图。

图8是本发明实施例中角点响应值计算的结构示意图。

图9是本发明实施例中角点判别的结构示意图。

具体实施方式

本发明提出一种利用并行计算加速的双目视觉惯性里程计方法，下面结合附图和具体实施例，对本发明进行详细说明。

本发明提出一种利用并行计算加速的双目视觉惯性里程计方法，实施例部署在EdgeBoard FZ3开发板上。该开发板长80mm，宽70mm，搭载有Xilinx Zynq UltraScale+MPSoCs(ZU3)系列芯片(型号为XAZU3EG-1SFVC784I)，处理器内置四核ARM Cortex-A53和可编程逻辑模块，板载DDR4的存储容量为2GB。

为了使该开发板能够实时运行双目视觉惯性里程计，本实施例同时使用了四核ARM Cortex-A53处理器和可编程逻辑块进行运算，其中，ARM处理器系统承担本方法中的步骤1)、步骤3)到步骤21)的计算任务，可编程逻辑块承担本方法中的步骤2)的计算任务。图1为本实施例的整体硬件结构示意图，展示了各模块的连接关系和各模块通信所遵守的总线协议。如图1所示，本实施例包含可编程逻辑部分和处理器系统两部分，可编程逻辑部分的核心是编程生成的角点检测模块，它从内存中读取像素流，经过运算后，将提取的角点坐标写入到内存中。为了对该实施例进行验证，采用EuRoC数据集的多条序列，对该实施例的耗时、功耗和精度进行测试。测试结果表明，针对大小为752×480的双目灰度图输入，图像处理部分的用时仅为42ms，最高双目帧率可达23帧/秒，满足实时性要求。在双目视觉惯性里程计运行期间，对整个开发板的功耗进行测量，平均功耗约为5.5W。将本实施例估计的飞行器轨迹与EuRoC数据集提供的真值进行对比，相对轨迹误差约为0.2％，满足定位精度的要求。图2展示了本实施例对EuRoC数据集中的V1_02_medium序列的估计轨迹相对真值的偏差，其中，实线为本方法实时估计的无人机在三维空间内的轨迹，虚线为EuRoC数据集提供的毫米级精度的无人机真实轨迹，可以看到两者非常接近，说明本方法的定位效果很好。

本发明提出一种利用并行计算加速的双目视觉惯性里程计方法，所有步骤被分配到两个线程中执行。其中，步骤1)到步骤9)在线程1中执行，步骤10)到步骤21)在线程2中执行。两个线程并行执行(从第2帧开始)。该方法整体流程如图3所示，其中采用并行计算加速的图像处理部分(即线程1)的流程如图4所示，线程1中步骤2)(角点检测，在FPGA上执行)、步骤3)到步骤4)(光流跟踪，在CPU上执行)并行执行。该方法包括以下步骤：

1)从惯性测量单元(IMU)读取IMU数据。从双目相机读取双目图像数据并作为线程1的当前帧(每帧双目图像包含一张左目图像和一张右目图像，均为8位灰度图，两张图像尺寸大小一致且无特殊要求，本实施例的尺寸为752×480)。将当前帧的右目图像和IMU数据暂存，等待步骤6)和步骤8)的处理。将当前帧的左目图像发送给FPGA。若该当前帧是第1帧，则设置一个初始为空的特征点列表并作为当前特征点列表(特征点列表对应于一帧图像，包含了该图像中所有跟踪中的特征点的左目坐标和右目坐标)，设置特征点列表的最大长度为N，N为正整数，本实施例设置为500；若当前帧不是第1帧，使用已经运行步骤更新后的当前特征点列表。

2)在FPGA上，利用角点检测方法，对线程1的当前帧的左目图像进行Harris角点提取，得到线程1的当前帧左目图像的角点坐标数组。具体步骤如下：

2-1)CPU在内存中设置一个角点坐标数组，初始为空，数组长度为N，本实施例为500，并进行角点检测模块配置(即图像尺寸设置为实际图像尺寸，角点判别阈值设置为1e-4)，并启动DMA传输和角点检测，以8位像素流的形式，将线程1当前帧的左目图像传送给位于FPGA的角点检测模块(该模块经由编程生成)。设置一个角点数目寄存器，存储已提取到的角点数目，避免提取的角点数目超过角点坐标数组的长度，初始值为0，最大值为N，本实施例为500。

2-2)角点检测模块从CPU传来的像素流中逐个获取线程1当前帧的左目图像的像素并作为当前像素。对每个当前像素，将该左目图像与两个方向的Sobel算子进行卷积，求出当前像素处的图像梯度[g_x，g_y]。其中，g_x为当前像素处沿行方向的图像梯度，g_y为当前像素处沿行方向的图像梯度，沿行方向的Sobel算子为

沿列方向的Sobel算子为

图5是本发明实施例中左目图像与Sobel算子进行卷积的结构示意图。如图5所示，两次卷积同时执行，其硬件实现包含三个行缓存、3×3卷积窗口(采用九个寄存器实现)、多个加法器和乘法器。当新像素到来，会被放入第三行行缓存的对应位置。与此同时，对应列的像素上移，并被取出，成为3×3卷积窗口内的最左列，其余列依次右移。由于每次移入的列编号比之前多1，按照这样的操作，3×3卷积窗口就会存储左目图像中连续的3×3像素块。块内的像素值同时与两个方向的Sobel算子对应位置的数值相乘，可以获得该像素处沿行方向和沿列方向的图像梯度。

2-3)根据步骤2-2)得到的当前像素处的图像梯度[g_x，g_y]，计算当前像素处的自相关矩阵

自相关矩阵含有三个独立分量A、B和C。针对它们的计算同时进行。具体计算方法见式(1)到式(3)，图6是本发明实施例中自相关矩阵计算的结构示意图。图6中，该步骤以图像梯度为输入量，将输入量连接到三个乘法器的输入端，乘法器的输出就是A、B和C。

A＝g_x ² (1)

B＝g_xg_y (2)

C＝g_y ² (3)

2-4)对步骤2-3)得到的自相关矩阵分量A、B和C分别进行均值滤波。均值滤波的计算方法是将A、B和C分别与大小为5×5的所有值为1的矩阵进行卷积。如图7所示，针对三分量的卷积同时进行，卷积操作的硬件实现与步骤2-2)类似。该步骤的输出为当前像素对应的滤波后的自相关矩阵分量A′、B′和C′。

2-5)利用步骤2-4)得到的A′、B′和C′，计算当前像素处的角点响应值R。

R的计算方法见式(4)。该步骤的结构示意图为图8。在图8中，该步骤以滤波后的自相关矩阵分量A′、B′和C′为输入量，通过乘法器、加法器等完成式(4)的运算操作，最终输出角点响应值R。

R＝A′*C′-B′²-0.04*(A′+C′)² (4)

2-6)依据步骤2-5)求出的角点响应值，判断当前像素是否为角点并得到相应的角点判别值。

角点判别条件包括两条：判定当前像素的角点响应值是否高于步骤2-1)中设置的角点判别阈值，以及判定当前像素的角点响应值是否高于邻近像素的角点响应值(邻近像素是指以当前像素为中心的大小为15×15的矩形区域内除当前像素外的所有像素)。

如图9所示，角点判别的硬件实现包含15个行缓存、15×15的像素窗口和多个比较器。当新的角点响应值到来，会被放入第15行行缓存的对应位置。与此同时，对应列的像素上移，并被取出，成为15×15像素窗口内的最左列，其余列依次右移。将中心位置的角点响应值和像素窗口内的其他角点响应值以及角点判别阈值进行比较。

若当前像素的角点响应值可以同时满足两条角点判别条件，则标记当前像素的角点判别值为1。若任一条件不满足，则标记当前像素的角点判别值为0。

2-7)若步骤2-6)得到的当前像素的角点判别值为1且角点数目寄存器的值小于500，则将当前像素的行列坐标记录到内存中的角点坐标数组中，更新角点坐标数组，同时角点数目寄存器的值加1。若步骤2-6)得到的当前像素的角点判别值为0或角点数目寄存器的值不小于500，不进行任何操作。

对当前帧的左目图像中的所有像素的角点判别值判定完毕后，线程1当前帧左目图像的角点坐标数组更新完毕，告知CPU已完成角点提取。

3)对线程1的当前帧进行判定：若该当前帧是第1帧，则直接进入步骤5)。若该当前帧不是第1帧，则进入步骤4)。

4)在线程1的前一帧左目图像和当前帧左目图像间进行光流跟踪，计算当前特征点列表中的特征点在当前帧左目图像中的坐标，并得到线程1当前帧最终跟踪成功的特征点集合。具体步骤如下：

4-1)对线程1当前帧左目图像进行高斯滤波和降采样，生成该当前帧左目图像的图像金字塔(层数为三层)。

4-2)将步骤4-1)的图像金字塔中的每层图像分别与Scharr算子进行卷积，计算每层图像对应的图像梯度图。

4-3)根据线程1前一帧左目图像的图像金字塔和图像梯度图、当前帧左目图像的图像金字塔和图像梯度图(在步骤4-1)和步骤4-2)中生成)，使用Lucas-Kanade光流法，对当前特征点列表中的特征点进行跟踪(此时当前特征点列表中所有特征点都是前一帧的特征点，记录了其左目坐标和右目坐标，其中右目坐标可能被标记为未获取，但左目坐标一定有)，得到当前帧左目图像初步跟踪成功的特征点集合，该集合包含了当前特征点列表中每个初步跟踪成功的特征点在当前帧左目图像中的坐标。

4-4)利用步骤4-3)得到的当前帧左目图像初步跟踪成功的特征点集合，使用Lucas-Kanade光流法，依据该集合中每个特征点在当前帧左目图像中的坐标(在步骤4-3)中生成)，计算该特征点在前一帧左目图像中的坐标。这一步的计算同样需要前一帧左目图像的图像金字塔和图像梯度图(在处理前一帧图像的步骤4-1)和步骤4-2)中生成)、当前帧左目图像的图像金字塔和图像梯度图(在步骤4-1)和步骤4-2)中生成)。若计算得到的坐标和该特征点的原始坐标的距离大于1个像素，则认为该特征点跟踪失败，将该特征点从当前帧左目图像初步跟踪成功的特征点集合中删除。

对步骤4-3)得到的集合中的所有特征点处理完毕后，得到线程1当前帧左目图像最终跟踪成功的特征点集合，该集合包含了当前特征点列表中每个跟踪成功的特征点在当前帧左目图像中的坐标。

5)依据步骤2)得到的当前帧左目图像的角点坐标数组和步骤4)得到的线程1当前帧最终跟踪成功的特征点集合，更新当前特征点列表中的左目坐标。具体步骤如下：

5-1)生成一张与线程1当前帧左目图像相同大小的二值图像，并将该二值图像的每个像素的初始值设置为0。

5-2)判定：如果没有进行步骤4)，则进入步骤5-5)，否则，进入步骤5-3)。

5-3)从步骤4)得到的线程1当前帧最终跟踪成功的特征点集合中依次取出每个特征点坐标(即该特征点在线程1当前帧左目图像中的坐标)。每次取出一个特征点的坐标后，查询二值图像中该特征点坐标对应位置的像素值是否为0。若二值图像中该对应位置的像素值为0，则将当前特征点列表中该特征点对应的左目坐标更新为该特征点坐标，并以该特征点坐标在二值图像中的对应位置为圆心，30个像素为半径，绘制圆，将圆内的所有二值图像像素值(包括圆心位置的像素值)更新为1。若二值图像中该对应位置的像素值为1，则不进行任何操作。

5-4)删除特征点列表中所有未在步骤5-3)中更新的特征点(被删除的特征点包括线程1当前帧未跟踪成功的特征点和因二值图像像素值为1而未被更新的特征点)，更新当前特征点列表。

5-5)从步骤2)得到的当前帧左目图像的角点坐标数组中依次取出每个角点坐标。每次取出一个角点坐标后，查询二值图像中该角点坐标对应位置的像素值是否为0。若该对应位置的像素值为0且当前特征点列表的长度低于500，则将该角点坐标作为一个新的特征点的左目坐标并加入到当前特征点列表中，更新当前特征点列表，然后以该角点坐标在二值图像中的对应位置为圆心，30个像素为半径，绘制圆，将圆内的所有二值图像像素值(包括圆心位置的像素值)更新为1。若二值图像中该对应位置的像素值为1，则不进行任何操作。

对当前帧左目图像的角点坐标数组中所有角点坐标处理完毕后，得到更新左目坐标后的当前特征点列表，其中该更新后的当前特征点列表中特征点为当前帧左目图像的特征点。

6)在线程1当前帧左目图像和线程1当前帧右目图像间进行光流跟踪，获得该当前帧左目图像的特征点(即步骤5)更新后的特征点列表中的特征点)在该当前帧右目图像中的坐标，得到初始更新完毕的当前特征点列表。具体步骤如下：

6-1)对线程1当前帧右目图像进行高斯滤波和降采样，生成该当前帧右目图像的图像金字塔(层数为三层)。

6-2)将步骤6-1)得到的图像金字塔中的每层图像与Scharr算子进行卷积运算，计算每层图像对应的图像梯度图。

6-3)根据线程1当前帧左目图像的图像金字塔和图像梯度图、当前帧右目图像的图像金字塔和图像梯度图(在步骤6-1)和步骤6-2)中生成)，使用Lucas-Kanade光流法，对步骤5)更新后的当前特征点列表中的特征点进行跟踪，得到当前帧右目图像初步跟踪成功的特征点集合，该集合包含了当前特征点列表中每个初步跟踪成功的特征点在当前帧右目图像中的坐标。

6-4)对步骤6-3)中得到的当前帧右目图像初步跟踪成功的特征点集合，使用Lucas-Kanade光流法，依据该集合中的每个特征点在当前帧右目图像中的坐标(在步骤6-3)中生成)，计算出该特征点在当前帧左目图像中的坐标。这一步的计算同样需要当前帧左目图像的图像金字塔和图像梯度图(在步骤4-1)和步骤4-2)中生成)、当前帧右目图像的图像金字塔和图像梯度图(在步骤6-1)和步骤6-2)中生成)。若计算得到的坐标和该特征点的原始左目坐标的距离大于1个像素，则认为该特征点跟踪失败，将该特征点从当前帧右目图像初步跟踪成功的特征点集合中删除。对步骤6-3)得到的集合中的所有特征点处理完毕后，得到线程1当前帧右目图像最终的跟踪成功的特征点集合，该集合包含了当前特征点列表中每个最终跟踪成功的特征点在当前帧右目图像中的坐标。

6-5)对步骤6-4)得到的线程1当前帧右目图像最终跟踪成功的特征点集合中的每个特征点，将当前特征点列表中该特征点对应的右目坐标修改为步骤6-3)得到的该特征点在当前帧右目图像中的坐标。将当前特征点列表中的其余特征点的右目坐标标记为未获取。修改完毕后，得到初始更新完毕的当前特征点列表。

7)对步骤6)得到的初始更新完毕的当前特征点列表中每个特征点的左右目坐标进行畸变矫正，更新该特征点列表中每个特征点的左右目坐标(除了未获取的右目坐标)，得到最终更新完毕的当前特征点列表。

8)对线程1的当前帧判定：若该当前帧是第1+3i帧(i为自然数，i＝0,1,2…)，则将该当前帧对应的步骤7)更新完毕的当前特征点列表发送给线程2，进入步骤9)；否则，不进行任何操作，进入步骤9)。

9)对线程1的当前帧判定：若该当前帧是第4+3i帧(i为自然数，i＝0,1,2…)，则将第1+3i帧和第4+3i帧间的所有IMU数据发送给线程2，然后当双目相机获取新一帧图像时，重新返回步骤1)；否则，不进行任何操作，然后当双目相机获取新一帧图像时，重新返回步骤1)。

10)从线程1获取IMU数据和当前特征点列表。将所获取的当前特征点列表对应的双目图像作为线程2的当前帧。

对线程2的当前帧进行判定：若该当前帧是第1帧，则设置大小为10帧的滑动窗口，用以保存线程2中每帧图像对应的当前特征点列表和任意相邻两帧间的IMU预积分结果。否则，使用经已运行步骤更新后的滑动窗口。将IMU数据暂存，等待后续步骤的处理。

11)对当前滑动窗口内帧数进行判定：若滑动窗口内的帧数少于10帧，则转到步骤12)，进行双目视觉惯性里程计的初始化。否则，转到步骤17)，采用非线性优化算法，对滑动窗口内各帧对应的相机位姿进行估计。

12)将线程2的当前帧(在步骤10)中获取)放入滑动窗口中，作为滑动窗口内的最新帧。对于该当前帧对应的特征点列表中左右目同时观测的特征点(即该特征点同时具有左目坐标和右目坐标，这样的特征点为该当前帧对应的当前特征点列表中的一部分)进行三角化，求出左右目同时观测的特征点的深度。

13)使用PnP算法(N点估计法)，依据步骤12)中已三角化的特征点的2D-3D匹配关系，计算线程2的当前帧对应的相机位姿(相机位姿定义为双目相机中左目图像传感器的位姿)。

14)若滑动窗口内的帧数大于1，则对滑动窗口中最新两帧间的IMU数据进行预积分，并将该预积分结果存储于滑动窗口中，然后进入步骤15)。否则，不进行任何操作，直接进入步骤15)。

下面介绍任意两帧间的IMU预积分方法。将滑动窗口内第k帧时的IMU坐标系记为b_k，获取该帧图像的时间记为t_k。从第k帧图像到第k+1帧间的预积分值包括三项，分别为

和

其定义见式(5)到式(7)。其中，

为t时刻的加速度计示数，

为t时刻陀螺仪给出的角速度示数。

和

分别是t时刻加速度计的偏置值和陀螺仪的偏置值。n_a和n_w分别是加速度的噪声和角速度的噪声。

是从t时刻的IMU坐标系到第k帧时的IMU坐标系的旋转矩阵。Ω(·)是针对三维向量的运算符，其定义见式(8)，w_x，w_y，w_z分别是w沿IMU坐标系的x轴、y轴和z轴方向的分量。

15)若滑动窗口内的帧数少于10帧，则重新返回步骤10)，从线程1获取新的IMU数据和特征点列表。否则，转到步骤16)。

16)将滑动窗口内各帧的相机位姿与IMU预积分值对齐，求解IMU中的陀螺仪的偏置初始值。

17)计算滑动窗口内的最新帧左目图像相对滑动窗口内的次新帧(即最新帧的前一帧)左目图像的平均视差量。平均视差量的计算方法是遍历两张左目图像共有的所有特征点，对共有的所有特征点在两幅图像的坐标变化量进行累加，最后除以共有的特征点的总个数，作为这两张图像的平均视差量。

18)当步骤17)得到的平均视差量高于10个像素时，则认为滑动窗口内的最新帧和次新帧间存在明显运动，标记滑动窗口内的最新帧为关键帧。否则，标记滑动窗口内的最新帧为非关键帧。

19)取出滑动窗口内的最新帧和线程2的当前帧间的所有IMU数据，进行IMU预积分(IMU预积分的计算方法见步骤14))。

20)构建与线程2的当前帧关联的视觉误差项和IMU误差项，添加入目标函数L。其中，目标函数中的优化变量X包含了滑动窗口内所有帧的设备状态和所有被滑动窗口内的多帧(多帧指大于等于2帧)观测的特征点的逆深度(此类特征点的数目记为m)。将滑动窗口内第k帧的设备状态记为x_k(k＝1，...10)，将线程2的当前帧的序号记为k＝11，则线程2的当前帧的设备状态记为x₁₁。每一帧的设备状态包含了该帧时IMU相对世界坐标系的位置

速度

旋转

(四元数形式)、加速度计偏置

和陀螺仪偏置

(k＝1，...11)。第j个特征点的逆深度记为λ_j(j＝1，…m)，它定义为该特征点在第一个观测到该特征点的左目图像中的深度的倒数。优化变量X的表达式如下：

X＝[x₁，x₁，…x₁₁，λ₀，λ₁，…λ_m] (9)

目标函数L定义为：

其中，式(11)中等式右边第一项为边缘化产生的先验信息项。第二项为IMU误差项。其中，z_IMU，k是第k帧到第k+1帧间的IMU预积分值，P_k是IMU误差项的协方差矩阵。第三项是视觉误差项。其中，C_k是第k帧所观测到的所有特征点的编号集合，z_CAM，k，j是通过光流跟踪确定的第j个特征点在第k帧左目和右目图像上的坐标，P_C是视觉误差项的协方差矩阵，按照下式计算，f代表相机焦距：

将IMU预积分值和帧间位姿变化量求差，可以得到IMU误差项。其具体计算方式见式(13)。式(13)中，

代表从世界坐标系到第k帧的IMU坐标系的旋转矩阵，g^w为重力向量在世界坐标系下的表达，Δt_k为第k帧和第k+1帧的时间间隔。

和

为从第k帧到第k+1帧的IMU预积分值(在步骤14)和步骤19)中获得)。[·]_xyz代表取四元数的虚数分量。

代表四元数乘法。

通过求取m个特征点在左目图像的投影位置和光流跟踪给出的实际位置的差距，可以得到视觉误差项。其具体计算方式见式(14)。对于左右目同时观测到的特征点，其还包含右目的视觉误差项，其计算需要进行先将特征点坐标从右目坐标系转换到左目坐标系，之后采用式(14)。式(14)中，

是通过光流跟踪确定的编号为j的特征点在第k帧的相机归一化坐标系中的坐标，

是编号为j的特征点在第k帧的相机坐标系中的坐标，||·||是取模运算。取一个与

相垂直的平面，其上任意选取一组单位正交基，记作

b_x和b_y分别是

在第k帧的相机坐标系中的表达。

采用高斯-牛顿法(一种非线性优化方法)，最小化目标函数L，获得优化变量X的增量，并对优化变量X进行更新。

21)对更新后的优化变量X进行边缘化，以维持恒定的滑动窗口长度。若滑动窗口内的最新帧为关键帧，则边缘化滑动窗口内的最老帧所对应的设备状态变量和最老帧所观测的特征点对应的逆深度变量，并将这两种变量的估计值转化为先验信息，加入到目标函数L中，然后将线程2的当前帧作为最新帧加入滑动窗口，滑动窗口更新完毕，返回步骤10)。

若滑动窗口内的最新帧不是关键帧，则直接舍弃滑动窗口内的最新帧和该帧的对应误差项，但保留滑动窗口内的最新帧和次新帧间的IMU预积分结果，将线程2的当前帧作为最新帧加入滑动窗口，滑动窗口更新完毕，重新返回步骤10)。

Claims (1)

1.一种利用并行计算加速的双目视觉惯性里程计方法，该方法所有步骤被分配到两个线程中执行；其中，步骤1)到步骤9)在线程1中执行，步骤10)到步骤21)在线程2中执行；从第2帧图像开始，两个线程并行执行；在线程1中，在可编程逻辑块FPGA上执行的步骤2)与在CPU执行的步骤3)到步骤4)并行执行；其特征在于，该方法包括以下步骤：

1)从惯性测量单元IMU读取IMU数据，从双目相机读取双目图像数据并作为线程1的当前帧，其中每帧双目图像包含一张左目图像和一张右目图像；将该当前帧的左目图像发送给FPGA；

当该当前帧为第1帧时，设置一个初始为空的特征点列表并作为当前特征点列表，令特征点列表最大长度为N，N为正整数；

2)在FPGA上，利用角点检测方法，对线程1的当前帧的左目图像进行Harris角点提取，得到线程1当前帧左目图像的角点坐标数组；具体步骤如下：

2-1)设置一个角点坐标数组，初始为空，数组长度为N；设置角点判别阈值；设置一个角点数目寄存器，初始值为0，最大值为N；

2-2)在FPGA中，依次提取线程1当前帧的左目图像的像素并作为当前像素；对每个当前像素，将该左目图像与两个方向的Sobel算子进行卷积，求出当前像素处的图像梯度[g_x，g_y]；其中，g_x为当前像素处沿行方向的图像梯度，g_y为当前像素处沿行方向的图像梯度，沿行方向的Sobel算子为

沿列方向的Sobel算子为

2-3)根据步骤2-2)得到的当前像素处的图像梯度[g_x，g_y]，计算当前像素处的自相关矩阵

其中，自相关矩阵的三个分量A、B和C，计算表达式如式(1)至式(3)所示：

A＝g_x ² (1)

B＝g_xg_y (2)

C＝g_y ² (3)

2-4)对步骤2-3)得到的自相关矩阵三个分量A、B和C分别进行均值滤波，得到滤波后的自相关矩阵分量A′、B′和C′；

2-5)利用步骤2-4)得到的A′、B′和C′，计算当前像素处的角点响应值R，表达式如下：

R＝A′*C′-B′²-0.04*(A′+C′)² (4)

2-6)依据步骤2-5)求出的角点响应值，判断当前像素是否为角点并得到相应的角点判别值；

角点判别条件包括两条：

2-6-1)判定当前像素的角点响应值是否高于步骤2-1)中设置的角点判别阈值；2-6-2)判定当前像素的角点响应值是否高于邻近像素的角点响应值，其中邻近像素是指以当前像素为中心的大小为15×15的矩形区域内除当前像素外的所有像素；

若当前像素的角点响应值同时满足两条角点判别条件，则标记当前像素的角点判别值为1；若任一条件不满足，则标记当前像素的角点判别值为0；

2-7)若步骤2-6)得到的当前像素的角点判别值为1且角点数目寄存器的值小于500，则将当前像素的行列坐标记录到角点坐标数组中，更新角点坐标数组，同时角点数目寄存器的值加1；若步骤2-6)得到的当前像素的角点判别值为0或角点数目寄存器的值不小于500，则不进行任何操作；

对当前帧的左目图像中的所有像素的角点判别值判定完毕后，线程1当前帧左目图像的角点坐标数组更新完毕；

3)对线程1的当前帧进行判定：若该当前帧是第1帧，则直接进入步骤5)；若该当前帧不是第1帧，则进入步骤4)；

4)在线程1的前一帧左目图像和当前帧左目图像间进行光流跟踪，计算当前特征点列表中的特征点在当前帧左目图像中的坐标，并得到线程1当前帧最终跟踪成功的特征点集合；具体步骤如下：

4-1)对线程1当前帧左目图像进行高斯滤波和降采样，生成该当前帧左目图像的图像金字塔，该图像金字塔层数为三层；

4-2)将步骤4-1)的图像金字塔中的每层图像分别与Scharr算子进行卷积，计算每层图像对应的图像梯度图；

4-3)根据线程1前一帧左目图像的图像金字塔和图像梯度图、当前帧左目图像的图像金字塔和图像梯度图，使用Lucas-Kanade光流法，对当前特征点列表中的特征点进行跟踪，得到当前帧左目图像初步跟踪成功的特征点集合；

4-4)利用步骤4-3)得到的当前帧左目图像初步跟踪成功的特征点集合，使用Lucas-Kanade光流法，依据该集合中每个特征点在当前帧左目图像中的坐标，计算该特征点在前一帧左目图像中的坐标；

若计算得到的坐标和该特征点的原始坐标的距离大于1个像素，则该特征点跟踪失败，将该特征点从当前帧左目图像初步跟踪成功的特征点集合中删除；

对步骤4-3)得到的集合中的所有特征点处理完毕后，得到线程1当前帧左目图像最终跟踪成功的特征点集合，该集合包含了当前特征点列表中每个跟踪成功的特征点在当前帧左目图像中的坐标；

5)依据步骤2)得到的当前帧左目图像的角点坐标数组和步骤4)得到的线程1当前帧最终跟踪成功的特征点集合，更新当前特征点列表中的左目坐标；具体步骤如下：

5-1)生成一张与线程1当前帧左目图像相同大小的二值图像，并将该二值图像的每个像素的初始值设置为0；

5-2)判定：如果线程1的当前帧没有进行步骤4)，则进入步骤5-5)，否则，进入步骤5-3)；

5-3)从步骤4)得到的线程1当前帧最终跟踪成功的特征点集合中依次取出每个特征点坐标，该坐标为该特征点在线程1当前帧左目图像中的坐标；每次取出一个特征点的坐标后，查询二值图像中该特征点坐标对应位置的像素值是否为0；若二值图像中该对应位置的像素值为0，则将当前特征点列表中该特征点对应的左目坐标更新为该特征点坐标，并以该特征点坐标在二值图像中的对应位置为圆心，30个像素为半径，绘制圆，将圆内的所有二值图像像素值更新为1；若二值图像中该对应位置的像素值为1，则不进行任何操作；

5-4)删除当前特征点列表中所有未在步骤5-3)中更新左目坐标的特征点，更新当前特征点列表；

5-5)从步骤2)得到的当前帧左目图像的角点坐标数组中依次取出每个角点坐标；每次取出一个角点坐标后，查询二值图像中该角点坐标对应位置的像素值是否为0；若该对应位置的像素值为0且当前特征点列表的长度低于设定的最大长度，则将该角点坐标作为一个新的特征点的左目坐标并加入到当前特征点列表中，更新当前特征点列表，然后以该角点坐标在二值图像中的对应位置为圆心，30个像素为半径，绘制圆，将圆内的所有二值图像像素值更新为1；若二值图像中该对应位置的像素值为1，则不进行任何操作；

对当前帧左目图像的角点坐标数组中所有角点坐标处理完毕后，得到更新左目坐标后的当前特征点列表，其中该更新后的当前特征点列表中的特征点为当前帧左目图像的特征点；

6)在线程1当前帧左目图像和线程1当前帧右目图像间进行光流跟踪，获得该当前帧左目图像的特征点在该当前帧右目图像中的坐标，得到初始更新完毕的当前特征点列表；具体步骤如下：

6-1)对线程1当前帧右目图像进行高斯滤波和降采样，生成该当前帧右目图像的图像金字塔，该图像金字塔层数为三层；

6-2)将步骤6-1)得到的图像金字塔中的每层图像与Scharr算子进行卷积运算，计算每层图像对应的图像梯度图；

6-3)根据线程1当前帧左目图像的图像金字塔和图像梯度图、当前帧右目图像的图像金字塔和图像梯度图，使用Lucas-Kanade光流法，对步骤5)更新后的当前特征点列表中的特征点进行跟踪，得到当前帧右目图像初步跟踪成功的特征点集合；

6-4)对步骤6-3)中得到的当前帧右目图像初步跟踪成功的特征点集合，使用Lucas-Kanade光流法，依据该集合中的每个特征点在当前帧右目图像中的坐标，计算该特征点在当前帧左目图像中的坐标；

若计算得到的坐标和该特征点的原始左目坐标的距离大于1个像素，则该特征点跟踪失败，将该特征点从当前帧右目图像初步跟踪成功的特征点集合中删除；

对步骤6-3)得到的集合中的所有特征点处理完毕后，得到线程1当前帧右目图像最终的跟踪成功的特征点集合，该集合包含了当前特征点列表中每个最终跟踪成功的特征点在当前帧右目图像中的坐标；

6-5)对步骤6-4)得到的线程1当前帧右目图像最终跟踪成功的特征点集合中的每个特征点，将当前特征点列表中该特征点对应的右目坐标修改为步骤6-3)得到的该特征点在当前帧右目图像中的坐标；将当前特征点列表中的其余特征点的右目坐标标记为未获取；修改完毕后，得到初始更新完毕的当前特征点列表；

7)对步骤6)得到的初始更新完毕的当前特征点列表中每个特征点的左右目坐标进行畸变矫正，更新该特征点列表中每个特征点的左右目坐标，得到最终更新完毕的当前特征点列表；

8)对线程1的当前帧判定：若该当前帧是第1+3i帧，其中，i为自然数，i＝0,1,2…，则将该当前帧对应的步骤7)更新完毕的当前特征点列表发送给线程2，进入步骤9)；否则，不进行任何操作，进入步骤9)；

9)对线程1的当前帧判定：若该当前帧是第4+3i帧，其中i为自然数，i＝0,1,2…，则将第1+3i帧和第4+3i帧间的所有IMU数据发送给线程2，然后当双目相机获取新一帧图像时，重新返回步骤1)；否则，不进行任何操作，然后当双目相机获取新一帧图像时，重新返回步骤1)；

10)从线程1获取IMU数据和当前特征点列表，将所获取的当前特征点列表对应的双目图像作为线程2的当前帧；

当线程2的当前帧为第1帧时，设置大小为10帧的滑动窗口，用以保存线程2中每帧图像对应的当前特征点列表和任意相邻两帧间的IMU预积分结果；

11)对当前滑动窗口内帧数进行判定：若滑动窗口内的帧数少于10帧，则转到步骤12)，进行双目视觉惯性里程计的初始化；否则，转到步骤17)，采用非线性优化算法，对滑动窗口内各帧对应的相机位姿进行估计；

12)将线程2的当前帧放入滑动窗口中，作为滑动窗口内的最新帧；对于该当前帧对应的特征点列表中同时具有左目坐标和右目坐标的特征点进行三角化，求出该特征点的深度；

13)利用步骤12)的结果，使用PnP算法，计算线程2的当前帧对应的相机位姿；

14)若滑动窗口内的帧数大于1，则对滑动窗口中最新两帧间的IMU数据进行预积分，并将该预积分结果存储于滑动窗口中，然后进入步骤15)；否则，不进行任何操作，直接进入步骤15)；

15)若滑动窗口内的帧数少于10帧，则重新返回步骤10)，从线程1获取新的IMU数据和特征点列表；否则，转到步骤16)；

16)将滑动窗口内各帧的相机位姿与IMU预积分值对齐，求解IMU中的陀螺仪的偏置初始值；

17)计算滑动窗口内的最新帧左目图像相对滑动窗口内的次新帧左目图像的平均视差量；

平均视差量的计算方法是遍历两张左目图像共有的所有特征点，对共有的所有特征点在两幅图像的坐标变化量进行累加，最后除以共有的特征点的总个数，作为这两张图像的平均视差量；

18)当步骤17)得到的平均视差量高于10个像素时，则标记滑动窗口内的最新帧为关键帧；否则，标记滑动窗口内的最新帧为非关键帧；

19)取出滑动窗口内的最新帧和线程2的当前帧间的所有IMU数据，进行IMU预积分；

20)构建与线程2的当前帧关联的视觉误差项和IMU误差项，添加入目标函数L；其中，目标函数中的优化变量X包含了滑动窗口内所有帧的设备状态和所有被滑动窗口内多于1帧观测的特征点的逆深度，其中被滑动窗口内多于1帧观测的特征点的数目记为m；

将滑动窗口内第k帧的设备状态记为x_k，k＝1，...10，将线程2的当前帧的序号记为k＝11,则线程2的当前帧的设备状态记为x₁₁；将世界坐标系记为w系，第k帧时的IMU坐标系记为b_k系；每一帧的设备状态包含了该帧时IMU相对世界坐标系的位置

速度

旋转

加速度计偏置

和陀螺仪偏置

第j个特征点的逆深度记为λ_j，j＝1，…m；则优化变量X的表达式如下：

X＝[x₁，x₁，...x₁₁，λ₀，λ₁，...λ_m] (5)

目标函数L定义为：

其中，式(7)中等式右边第一项为边缘化产生的先验信息项，第二项为IMU误差项，第三项是视觉误差项；z_IMU，k是第k帧到第k+1帧间的IMU预积分值，P_k是IMU误差项的协方差矩阵；C_k是第k帧所观测到的所有特征点的编号集合，z_CAM，k，j是通过光流跟踪确定的第j个特征点在第k帧左目和右目图像上的坐标，P_C是视觉误差项的协方差矩阵，按照下式计算，f代表相机焦距：

将IMU预积分值和帧间位姿变化量求差，得到IMU误差项，计算表达式如式(9)所示：

其中，

代表从世界坐标系到第k帧的IMU坐标系的旋转矩阵，g^w为重力向量在世界坐标系下的表达，Δt_k为第k帧和第k+1帧的时间间隔；

和

为从第k帧到第k+1帧的IMU预积分值；[·]_xyz代表取四元数的虚数分量，

代表四元数乘法；

通过求取m个特征点在左目图像的投影位置和光流跟踪给出的实际位置的差距，得到视觉误差项；

对于同时具有左目坐标和右目坐标的特征点，先将该特征点坐标从右目坐标系转换到左目坐标系，然后采用式(10)计算该特征点的视觉误差项：

其中，

是通过光流跟踪确定的编号为j的特征点在第k帧的相机归一化坐标系中的坐标，

是编号为j的特征点在第k帧的相机坐标系中的坐标，||·||是取模运算；取一个与

相垂直的平面，在该平面任意选取一组单位正交基，记作

b_x和b_y分别是

在第k帧的相机坐标系中的表达；

采用高斯-牛顿法最小化目标函数L，获得优化变量X的增量，并对优化变量X进行更新；

21)对更新后的优化变量X进行边缘化；

若滑动窗口内的最新帧为关键帧，则边缘化滑动窗口内的最老帧所对应的设备状态变量和最老帧所观测的特征点对应的逆深度变量，并将这两种变量的估计值转化为先验信息，加入到目标函数L中，然后将线程2的当前帧作为最新帧加入滑动窗口，滑动窗口更新完毕，然后重新返回步骤10)；

若滑动窗口内的最新帧不是关键帧，则舍弃滑动窗口内的最新帧和该帧的对应误差项，但保留滑动窗口内的最新帧和次新帧间的IMU预积分结果，将线程2的当前帧作为最新帧加入滑动窗口，滑动窗口更新完毕，然后重新返回步骤10)。

Images