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

权利要求书

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)。