CN102682448B - 一种基于双三视张量的立体视觉快速导航定位方法 - Google Patents

Abstract

本发明属于视觉导航定位技术领域，公开了一种基于双三视张量的立体视觉快速导航定位方法。该方法利用运动前后立体相机四幅图像的特征点图像坐标建立双三视张量运动约束对3D-3D位姿估计方法的初始运动参数进行优化，参数优化精确高。该方法采用ICCV2011提出的BRISK特征作为跟踪匹配的特征，该特征计算速度快，定位精度高，鲁棒性。立体匹配和前后帧匹配都采用局部描述符U-BRISK，该描述符计算速度快。由于U-BRISK描述符为二进制比特串，所以匹配速度也很快。立体匹配和前后帧匹配都采用U-BRISK描述符进行匹配，鲁棒性和定位的精确度都很高。

Description

一种基于双三视张量的立体视觉快速导航定位方法

技术领域

本发明属于视觉导航定位技术领域，涉及一种基于双三视张量的立体视觉快速导航定位方法，可以应用于智能移动机器人、自主导航车等领域。

背景技术

近年来国内外研究人员对移动机器人的定位方法与技术采用了各种定位传感器，研究出了许多不同的定位系统，主要有里程计法、惯性导航，全球定位导航。里程计法是移动机器人导航技术中最为广泛采用的定位方法。在移动机器人的车轮上装有光电编码器，通过对车轮转动的记录来计算出机器人位置和姿态。这种方法的最大缺点是车轮滑动引起的编码器读数错误。惯性导航法采用陀螺仪和加速度计来实现定位，陀螺仪测量旋转速度，加速度计测量加速度。根据测量值的一次积分和二次积分可得到机器人的位置信息。这种方法的缺点是惯导设备随时间的误差漂移太大。全球定位系统是用于室外移动机器人导航与定位的技术。它是一种以空间卫星为基础的高精度导航与定位系统。可是它在卫星信号封闭或受限的环境下，无法提供可靠的信息。比如在室内、水下、地下、战场、外星球等。

目前，随着计算速度和视觉传感器性价比的不断提高以及图像处理和视觉计算的飞速发展，基于视觉的导航定位技术受到了研究者的广泛关注。该方法依赖硬件设备少、对体积、功耗、载荷等要求低，适用于各种复杂未知环境，且定位精度高，该方法在“勇气”号与“机遇”号火星车上也得到了成功的应用。

视觉的导航定位技术仅依赖视觉输入进行运动估计，其关键是在二维图像序列中追踪视觉特征点进行机器人自身的运动估计。由于单目视觉定位存在尺度模糊性，因此，目前广泛采用立体视觉的定位方法。

立体视觉导航定位方法一般通过特征提取、立体匹配、特征跟踪、运动估计得出智能车运动的距离和方向信息。由于运动前后的特征点可以通过立体匹配的视差计算得到其三维坐标，所以目前的立体视觉运动估计算法一般都采用基于3D-3D特征对应的运动估计算法。3D-3D特征对应的运动估计算法一般采用线性或非线性最小二乘方法使估计的特征点和测量的特征点之间的3D距离最小，但由于特征点(特别对于远距离特征点)3D坐标重建受噪声影响较大，因此这种方法得到的定位精度不高。

发明内容

本发明的目的在于克服现有技术中的不足和缺点，提出一种基于双三视张量的立体视觉快速导航定位方法。该方法包括如下步骤：

步骤一、提取立体相机视频流当前帧左、右图像的BRISK特征点和各BRISK特征点对应图像位置的U-BRISK描述符(参见L.Stefan，C.Margarita，s.Roland.BRISK：Binary RobustInvariant Scalable Keypoints.In Proceedings of the IEEE International Conference on ComputerVision(ICCV)，2011)。

步骤二、对当前帧左、右图像的BRISK特征点集进行一一匹配，得到当前帧左、右图像BRISK特征点匹配对，并得到当前帧左、右图像BRISK特征点匹配对的3D坐标。

步骤三、对当前帧左图像或右图像和前一帧左图像或右图像的BRISK特征点集进行一一匹配，并联合步骤二得到前后帧立体图像特征匹配点集。

步骤四、采用RANSAC算法(随机采样一致性算法，参见M.A.Fischler and R.C.Bolles.Random sample consensus：a paradigm for model fitting with applications to image analysis and automatedcartography.Commun.ACM，1981，24(6)：381-395.)和基于SVD的位姿估计方法得到匹配内点和当前帧与前一帧之间的立体相机运动参数(即当前帧运动参数)；所述运动参数包括相对位置参数、相对姿态参数，所述相对位置参数为平移参数t，所述相对姿态参数为旋转参数R；

步骤五、建立双三视张量运动约束的非线性优化目标函数，采用Levenberg-Marquard算法对该目标函数当前帧运动参数进行迭代优化；运动参数的迭代初值为步骤四计算得到的运动参数值。

步骤六、利用前一帧的全局位姿参数和当前帧的运动参数计算得到当前帧的全局位姿参数，即相对于起始帧的位置和姿态，从而得到立体相机的导航定位信息。

该方法采用ICCV2011提出的BRISK特征作为跟踪匹配的特征，该特征计算速度快，定位精度高，鲁棒性。立体匹配和前后帧匹配都采用局部描述符U-BRISK，该描述符计算速度快。由于U-BRISK描述符为二进制比特串，所以匹配速度也很快。立体匹配和前后帧匹配都采用U-BRISK描述符进行匹配，鲁棒性和定位的精确度都很高。

传统的立体匹配采用如下的方法：首先采用极线校正方法对立体图像对进行校正，使左右图像对应的特征点位于同一水平扫描线上，然后，对采用基于SAD(或NCC)的立体匹配方法沿水平方向极线进行一维搜索匹配，从而得到第一图像的特征在第二图像上的对应图像坐标。该方法的缺点是：需要对立体视频流进行立体极线校正，对立体极线校正方法的精度依赖也较大。

由于初始运动参数精度不高，需要采用优化方法对其进行进一步的优化。传统的3D-3D优化方法在3D空间进行优化，但由于特征点(特别对于远距离特征点)3D坐标重建受噪声影响较大，因此这种方法得到的定位精度不高。本发明通过双三视张量运动约束方程直接采用立体相机的四个图像坐标进行运动参数优化，所以优化精度高。

在本发明的立体视觉快速导航定位方法中，所述步骤一包括以下具体步骤：

(1)、提取立体相机视频流当前帧左、右图像的BRISK特征点，得到当前帧左、右图像的BRISK特征点的图像坐标(u_L，k，v_L，k)、(u_R，k，v_R，k)；

(2)、计算在图像坐标(u_L，k，v_L，k)、(u_R，k，v_R，k)位置上的U-BRISK描述符；

所述步骤二包括以下步骤：

(1)、取当前帧左图像或右图像中的任一个特征点BRISK描述符，和另一图像中在视差阈值范围(0，L₁)内的特征点BRISK描述符，计算二者之间的汉明距离，如果汉明距离小于汉明阀值T时则接受这一对特征匹配点为匹配对，否则拒绝。

(2)、利用已标定的立体相机左相机和右相机内部参数矩阵K_L和K_R以及已标定的右相机相对左相机的旋转矩阵和平移向量R_r和t_r，采用线性最小二乘算法可得到当前帧左、右图像BRISK特征点的图像坐标(u_L，k，v_L，k)、(u_R，k，v_R，k)相对应的世界点3D坐标Y。

所述步骤三包括以下步骤：

(1)、取当前帧左图像或右图像中的任一个特征点U-BRISK描述符，和前一帧左图像或右图像中在视差阈值范围(-L₂，L₂)内的特征点U-BRISK描述符，计算二者之间的汉明距离，如果该汉明距离小于汉明阀值T时则接受这一对特征匹配点为匹配点，否则拒绝；

(2)、选择当前帧左、右图像特征匹配、前一帧左、右特征匹配、当前帧和前一帧图像特征匹配的公共匹配点集作为最后的前后帧立体图像特征匹配点集；设前一帧左、右图像BRISK特征点的图像坐标分别为(u_L，k-1，v_L，k-1)、(u_R，k-1，v_R，k-1)，设前一帧图像BRISK特征点的3D坐标为X。

所述步骤四包括以下步骤：

(1)、在所述步骤三得到的前后帧立体图像特征匹配点集中随机采样3对匹配点；

(2)、利用基于SVD的3D-3D位姿估计算法计算该3对匹配点的运动参数旋转矩阵R和平移向量t；

(3)、利用步骤(2)中获得的该运动参数R和t，计算所有的前后帧特征匹配对在该运动模型下的3D坐标误差，即：

|Y-(RX+t)|＜τ    (1)

该误差小于阀值τ的前后帧特征匹配对称为匹配内点，存储匹配内点、运动参数R和t以及匹配内点数；

(4)、重复步骤(1)、  (2)、  (3)多次，选取匹配内点数最多相对应的计算结果作为最后的匹配内点和初始运动参数R₀和t₀。

在本发明的方法中，步骤五所述的双三视张量运动约束的非线性优化目标函数为f(θ)＝(P(θ)-m)^T(P(θ)-m)，采用Levenberg-Marquard算法(参见袁亚湘、孙文瑜编著的《最优化理论与方法》，科学出版社)

…)对该目标函数中当前帧运动参数进行迭代优化的计算步骤如下：

(1)、计算 $\∂\mathrm{\θ}={(J^{T}J+\mathrm{\λI})}^{-1}{J}^T(P\left(\mathrm{\θ}\right)-m),$ 则 ${\mathrm{\θ}}_{\mathrm{new}}=\mathrm{\θ}+\∂\mathrm{\θ}.$

其中 $J=\frac{\∂\tilde{P}\left(\mathrm{\θ}\right)}{\∂\mathrm{\θ}}==\left(\begin{array}{ccc}\frac{\∂{\tilde{P}}_1\left(\mathrm{\θ}\right)}{{\∂\mathrm{\θ}}_1}& ...& \frac{\∂{\tilde{P}}_N\left(\mathrm{\θ}\right)}{{\∂\mathrm{\θ}}_6}\\ ...& ...& ...\\ \frac{\∂{\tilde{P}}_1\left(x\right)}{{\∂\mathrm{\θ}}_1}& ...& \frac{\∂{\tilde{P}}_N\left(x\right)}{{\∂\mathrm{\θ}}_6}\end{array}\right)$

(2)、若f(θ_new)＜f(θ)则转向(4)；否则进行(3)。

(3)、若停止计算，输出结果。否则置λ＝βλ，θ＝θ_new，转(2)。

(4)、若停止计算，输出结果。否则置λ＝λ/β，θ＝θ_new，转(2)。

在上述运动参数进行迭代优化步骤中，所述运动参数θ＝[q₁ q₂ q₃ x y z]^T为6维向量；其初始值为旋转矩阵R转化的四元数(q₁，q₂，q₃，q₄)，其中q₁，q₂，q₃为独立变量，所述平移参数t＝[x y z]^T。

在所述的非线性优化目标函数f(θ)＝(P(θ)-m)^T(P(θ)-m)中， $P\left(\mathrm{\θ}\right)={[{\tilde{P}}_1,{\tilde{P}}_2,...,{\tilde{P}}_n]}^T,$ m＝[m₁，m₂，...，m_n]^T。

其中 ${\tilde{P}}_i=\left[\begin{array}{c}{\tilde{P}}_{L,k}\\ {\tilde{P}}_{R,k}\end{array}\right]=\left[\begin{array}{c}{\tilde{u}}_{L,k}\\ {\tilde{v}}_{L,k}\\ {\tilde{u}}_{R,k}\\ {\tilde{v}}_{R,k}\end{array}\right],$ $m_i=\left[\begin{array}{c}{u}_{L,k}\\ v_{L,k}\\ u_{R,k}\\ u_{R,k}\end{array}\right].$ 所述 ${\tilde{P}}_{R,k}=\left[h_{R,1}{h}_{R,2}{h}_{R,3}\right]P_{L,k-1}$ 为双三视张量运动约束的点-线-点约束1；为双三视张量运动约束的点-线-点约束2；其中i＝1，2，3，i＝1，2，3，l_R，k-1为经过图像点(u_R，k-1，v_R，k-1)的直线，其向量坐标为[-1 -1 u_R，k-1+v_R，k-1]^T，P_L，k-1为特征点的齐次图像坐标[u_L，k-1 v_L，k-1 1]^T。

所述的T_R，i为前一帧左图像、前一帧右图像、当前帧右图像组成的三视张量：

$T_{R,i}=a_i{b}_4^T-b_4{a}_i^T,i=\mathrm{1,2,3}---\left(2\right)$

T_L，i为前一帧左图像、前一帧右图像、当前帧左图像组成的三视张量为：

$T_{L,i}=a_i{c}_4^T-b_4{c}_i^T,i=\mathrm{1,2,3}---\left(3\right)$

上式(2)、(3)中的a，b，c计算方式如下：

把立体相机的前一帧左相机图像坐标系选为世界坐标系，左相机的3*4的投影矩阵为M_L，k-1＝K_L[I|0]，则

前一帧右相机的投影矩阵：M_R，k-1＝K_R[R_r|t_r]＝[a₁ a₂ a₃ a₄]    (4)

当前帧左相机的投影矩阵：M_L，k＝K_L[R_L|t_L]＝[c₁ c₂ c₃ c₄]    (5)

上式中R_L＝R_r ^T·R，t_L＝R_r ^T·t-R_r ^T·t_r。

当前帧右相机的投影矩阵：M_R，k＝K_R[R|t]＝[b₁ b₂ b₃ b₄]    (6)

在本发明的立体视觉快速导航定位方法中，所述步骤六具体为：

利用拍摄当前帧前一帧相对于世界坐标系的旋转参数R和平移参数t、拍摄当前帧摄像头运动参数θ计算得到拍摄当前帧时摄像头相对于世界坐标系的旋转参数R和平移参数t；拍摄当前帧时摄像头相对于世界坐标系的旋转参数和平移参数即为立体相机6个自由度的3D定位信息。

综上所述，本发明具有的有益效果如下：(1)由于立体匹配采用局部特征匹配且采用双三视张量运动约束，所以不需要对立体相机进行立体极线校正，省去了很大的计算量。(2)直接利用运动前后立体相机四幅图像的特征点图像坐标建立双三视张量运动约束，参数优化更精确。(3)、采用了BRISK特征和U-BRISK描述符，该特征和描述符计算复杂度很低，U-BRISK描述符为二进制比特流，所以在特征匹配时采用Hamming距离，匹配速度快。(4)仅利用立体相机得到的视频流进行移动机器人的导航定位，具有完全的自主性。

附图说明

图1为立体匹配过程示意图；

图2为前后帧匹配过程示意图

图3为立体图像特征匹配结果

图4为前后帧图像特征匹配结果

图5为双三视张量运动约束原理示意图。

图6为本发明的导航定位结果

具体实施方式

为了使本发明的技术方案和优点更加清楚，下面结合附图对本发明做进一步的详细描述。

1、提取立体相机视频流当前帧左、右图像的BRISK特征点，得到当前帧左、右图像的BRISK特征点的图像坐标(u_L，k，v_L，k)和(u_R，k，v_R，k)。左图像共有n₁个BRISK特征点，右图像共有n₂个BRISK特征点；在相应图像坐标位置上的提取U-BRISK描述符；

2、取当前帧第一图像中的任一个特征点U-BRISK描述符，和第二图像中在视差阈值范围(0，60)内的特征点U-BRISK描述符，计算二者之间的汉明距离，如果汉明距离小于阀值80则接受这一对特征匹配点为匹配对，否则拒绝；所述第一图像即为左、右图像中任一图像，另一个为第二图像。图3给出了立体图像特征匹配结果。由实验可得：对640×480像素立体图像进行极线校正的时间为340ms，而本发明方法的立体匹配时间只有60ms。

3、利用已标定的立体相机左相机和右相机内部参数矩阵K_L和K_R以及已标定的右相机相对左相机的旋转矩阵和平移向量R_r和t_r，采用线性最小二乘算法可得到当前帧左、右图像BRISK特征点的图像坐标(u_L，k，v_L，k)、(u_R，k，v_R，k)相对应的世界点3D坐标Y。

4、(1)取当前帧第一图像中的任一个特征点U-BRISK描述符，和前一帧第一图像中在视差阈值范围(-80，80)内的特征点U-BRISK描述符，计算二者之间的汉明距离，如果汉明距离小于阀值90则接受这一对特征匹配点为匹配对，否则拒绝；图4给出了前后帧图像特征匹配结果。

(2)、选择当前帧左右特征匹配、前一帧左右特征匹配、当前帧和前一帧特征匹配的公共匹配点集作为最后的前后帧立体图像特征匹配点集。设前一帧左右BRISK特征的图像坐标分别为(u_L，k-1，v_L，k-1)、(u_R，k-1，v_R，k-1)。设前一帧BRISK特征的3D坐标为X。

5、(1)、在前后帧匹配点集中随机采样3对匹配点；

(2)、采用基于SVD的位姿估计算法计算该3对匹配点的运动参数旋转矩阵R和平移向量t；

(3)、利用步骤(2)中获得的该运动参数R和t，计算所有的前后帧特征匹配对在该运动模型下的3D坐标误差，即：

|Y-(RX+t)|＜τ

该误差小于阀值τ的前后帧特征匹配对称为匹配内点。存储匹配内点、运动参数R和t以及匹配内点数。

(4)、重复步骤(1)、(2)、(3)1000次，选取匹配内点数最多相对应的计算结果作为最后的匹配内点和初始运动参数R和t。

6、计算旋转矩阵R相对应的四元素表达(q₂，q₃，q₄)，加上步骤四的平移参数t＝[x y z]^T，则得到6维参数的运动向量θ＝[q₁ q₂ q₃ x y z]^T。

7、计算R_L＝R_r ^T·R，t_L＝R_r ^T·t-R_r ^T·t_r。

8、计算当前帧左相机的投影矩阵：[c₁ c₂ c₃ c₄]＝K_L[R_L|t_L]。

9、计算当前帧右相机的投影矩阵：[b₁ b₂ b₃ b₄]＝K_R[R|t]。

10、计算由前一帧左图像、前一帧右图像、当前帧右图像组成的三视张量：

$T_{R,i}=a_i{b}_4^T-b_4{a}_i^T,i=\mathrm{1,2,3}$

计算由前一帧左图像、前一帧右图像、当前帧左图像组成的三视张量：

$T_{L,i}=a_i{c}_4^T-b_4{c}_i^T,i=\mathrm{1,2,3}$

11、计算 $h_{R,i}=T_{R,i}^T{l}_{R,k-1},i=\mathrm{1,2,3}$

计算 $h_{L,i}=T_{L,i}^T{l}_{R,k-1},i=\mathrm{1,2,3}$

其中l_R，k-1＝[-1 -1 u_R，k-1+v_R，k-1]^T，P_L，k-1＝[u_L，k-1 v_L，k-1 1]^T

12、计算双三视张量运动约束的点-线-点约束1：

${\tilde{P}}_{R,k}=\left[\begin{array}{ccc}{h}_{R,1}& h_{R,2}& h_{R,3}\end{array}\right]P_{L,k-1}$

计算双三视张量运动约束的点-线-点约束2：

${\tilde{P}}_{L,k}=\left[\begin{array}{ccc}{h}_{L,1}& h_{L,2}& h_{L,3}\end{array}\right]P_{L,k-1}$

图5给出了双三视张量运动约束原理示意图。

13、计算 $J=\frac{\∂\tilde{P}\left(\mathrm{\θ}\right)}{\∂\mathrm{\θ}}==\left(\begin{array}{ccc}\frac{\∂{\tilde{P}}_1\left(\mathrm{\θ}\right)}{{\∂\mathrm{\θ}}_1}& ...& \frac{\∂{\tilde{P}}_N\left(\mathrm{\θ}\right)}{{\∂\mathrm{\θ}}_6}\\ ...& ...& ...\\ \frac{\∂{\tilde{P}}_1\left(x\right)}{{\∂\mathrm{\θ}}_1}& ...& \frac{\∂{\tilde{P}}_N\left(x\right)}{{\∂\mathrm{\θ}}_6}\end{array}\right)$

其中 $P\left(\mathrm{\θ}\right)={[{\tilde{P}}_1,{\tilde{P}}_2,...,{\tilde{P}}_n]}^T,$ m＝[m₁，m₂，...，m_n]^T， ${\tilde{P}}_i=\left[\begin{array}{c}{\tilde{P}}_{L,k}\\ {\tilde{P}}_{R,k}\end{array}\right]=\left[\begin{array}{c}{\tilde{u}}_{L,k}\\ {\tilde{v}}_{L,k}\\ {\tilde{u}}_{R,k}\\ {\tilde{v}}_{R,k}\end{array}\right],$ $m_i=\left[\begin{array}{c}{u}_{L,k}\\ v_{L,k}\\ u_{R,k}\\ u_{R,k}\end{array}\right]$

14、计算 $\∂\mathrm{\θ}={(J^{T}J+\mathrm{\λI})}^{-1}{J}^T(P\left(\mathrm{\θ}\right)-m).$

15、计算 ${\mathrm{\θ}}_{\mathrm{new}}=\mathrm{\θ}+\∂\mathrm{\θ}.$

16、若f(θ_new)＜f(θ)则转向步骤18；否则进行步骤17。

17、若转向步骤19。否则置λ＝βλ，θ＝θ_new，转入步骤6。

λ、β、ε分别取0.01、10和0.000001。

18、若转向步骤19。否则置λ＝λ/β，θ＝θ_new，转入步骤6。

19、利用拍摄当前帧前一帧相对于世界坐标系的旋转参数R和平移参数t、拍摄当前帧摄像头运动参数θ计算得到拍摄当前帧时摄像头相对于世界坐标系的旋转参数R和平移参数t；拍摄当前帧时摄像头相对于世界坐标系的旋转参数和平移参数即为立体相机6个自由度的3D定位信息。图6给出了本发明的定位结果。

Claims (4)

1.一种基于双三视张量的立体视觉快速导航定位方法，其特征在于，包括如下步骤：

步骤一、提取立体相机视频流当前帧左、右图像的BRISK 特征点和各BRISK 特征点对应图像位置的U-BRISK 描述符；

步骤二、对当前帧左、右图像的BRISK 特征点集进行一一匹配，得到当前帧左、右图像BRISK 特征点匹配对，并得到当前帧左、右图像BRISK 特征点匹配对的3D 坐标；

步骤三、对当前帧左、右图像和前一帧左、右图像的BRISK 特征点集进行一一匹配，并联合步骤二得到前后帧立体图像特征匹配点集；

步骤四、采用RANSAC 算法和基于SVD 的位姿估计方法得到匹配内点和当前帧与前一帧之间的立体相机运动参数；所述运动参数包括相对位置参数、相对姿态参数，所述相对位置参数为平移参数t，所述相对姿态参数为旋转参数R；

步骤五、建立双三视张量运动约束的非线性优化目标函数，采用Levenberg-Marquard算法对该目标函数当前帧运动参数进行迭代优化；运动参数的迭代初值为步骤四计算得到的运动参数值；

步骤六、利用前一帧的全局位姿参数和当前帧的运动参数计算得到当前帧的全局位姿参数，从而得到立体相机的导航定位信息；

所述步骤二包括以下步骤：

(1)、取当前帧左图像或右图像中的任一个BRISK 特征点对应图像位置的U-BRISK 描述符，和另一图像中在视差阈值范围(0，L₁) 内的BRISK 特征点对应图像位置的U-BRISK 描述符，计算二者之间的汉明距离，如果汉明距离小于汉明阈值T 时则接受这一对特征匹配点为匹配对，否则拒绝；

(2)、利用已标定的立体相机左相机和右相机内部参数矩阵K_L 和K_R 以及已标定的右相机相对左相机的旋转矩阵和平移向量R_r 和t_r，采用线性最小二乘算法可得到当前帧左、右图像BRISK 特征点的图像坐标(u_L，k，v_L，k)、(u_R，k，v_R，k) 相对应的3D 坐标Y。

2.根据权利要求1 所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，所述步骤一包括以下步骤：

(1)、提取立体相机视频流当前帧左、右图像的BRISK 特征点，得到当前帧左、右图像的BRISK 特征点的图像坐标(u_L，k，v_L，k)、(u_R，k，v_R，k) ；

(2)、计算在图像坐标(u_L，k，v_L，k)、(u_R，k，v_R，k) 位置上的U-BRISK 描述符。

 

3.根据权利要求1 所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于， 所述步骤三包括以下步骤：

(1)、取当前帧左图像或右图像中的任一个BRISK 特征点对应图像位置的U-BRISK 描述符，和前一帧左图像或右图像中在视差阈值范围(-L₂，L₂) 内的BRISK 特征点对应图像位置的U-BRISK 描述符，计算二者之间的汉明距离，如果该汉明距离小于汉明阈值T 时则接受这一对特征匹配点为匹配点，否则拒绝；

(2)、选择当前帧左、右图像特征匹配、前一帧左、右图像特征匹配、当前帧和前一帧图像特征匹配的公共匹配点集作为最后的前后帧立体图像特征匹配点集；设前一帧左、右图像BRISK特征点的图像坐标分别为(u_L，k-1，v_L，k-1)、(u_R，k-1，v_R，k-1)，设前一帧图像BRISK 特征点的3D坐标为X。

4.根据权利要求3所述的基于双三视张量的立体视觉快速导航定位方法，其特征在于，所述步骤四包括以下步骤：

(1)、在所述步骤三得到的前后帧立体图像特征匹配点集中随机采样3 对匹配点；

(2)、利用基于SVD 的3D-3D 位姿估计算法计算该3 对匹配点的旋转参数R 和平移参数t；

(3)、利用步骤(2)中获得的该旋转参数R和平移参数t，计算所有的前后帧特征匹配点对在运动模型下的3D坐标误差，即：

|Y-(RX+t)|＜τ       (1)

该误差小于阈值τ的前后帧特征匹配点对称为匹配内点，存储匹配内点、旋转参数R 和平移参数t 以及匹配内点数；

(4)、重复步骤(1)、(2)、(3)多次，选取匹配内点数最多相对应的计算结果作为最后的匹配内点和当前帧与前一帧之间的立体相机运动参数。