CN102607535B - 一种利用视差空间集束调整的高精度实时立体视觉定位方法 - Google Patents

Abstract

本发明提出了一种利用视差空间集束调整的高精度实时立体视觉定位方法。本发明仅利用立体相机得到的视频流进行移动机器人的导航定位，具有完全的自主性；本发明利用U-SURF描述符进行特征匹配，对运动过程中出现的光照变化、运动模糊、大的旋转和尺度变换具有较强的鲁棒性；本发明利用视差空间集束调整优化方法使自主导航车在长距离行进下也能够进行精度实时的定位，其具有对优化初始值依赖程度低，收敛速度快，数值稳定性高等优点，与利用集束调整的立体视觉定位方法相比，定位精度更高，速度更快。本发明整个立体视觉定位方法计算速度快，可在线实时运行。

Description

一种利用视差空间集束调整的高精度实时立体视觉定位方法

技术领域

本发明属于计算机视觉自主导航技术领域，涉及一种基于视差空间集束调整的高精度实时立体视觉定位方法，可以应用于自主移动机器人、智能车辆等领域。

背景技术

在未知环境中自主导航是各种移动机器人或智能车应用的关键任务之一。机器人在其长距离漫游过程中需要实时地获得自身的定位信息。目前，由于视觉传感器的性价比日益提高以及视觉计算的发展，基于视觉的导航定位方法正开始广泛运用于各个相关领域。视觉导航定位方法依赖硬件设备少、对体积、功耗、载荷等要求低，适用于各种复杂未知环境，并能够填补GPS卫星定位的盲区，有效纠正轮式里程计和惯性导航的累计误差。因此，采用视觉定位方法已经成为机器人自主导航的必然趋势。

实时立体视觉定位算法一般利用二帧之间的特征匹配信息进行运动估计，对运动估计参数进行级联计算出摄像机定位信息，这种方法使小的运动估计误差不断积累，从而导致长距离行进下定位精度不高。

经对现有技术的文献检索发现，E.Mouragnon等人于2006年《2006 Proceedings of theIEEE Conference on Computer Vision and Pattern Recognition》上发表的论文“Real-TimeLocalization and 3D Reconstruction”以及N.Sunderhauf等人于2006年《2006 Proceedings ofTowards Autonomous Robotic Systems》上发表的论文“Towards Using Bundle Adjustment forRobust Stereo Odometry in Outdoor Terrain”提出了利用集束调整进行实时视觉定位。集束调整采用公共坐标系统同时优化3D点和摄像机位姿参数，由于其可以利用多帧之间的特征匹配信息，所以作为最后一步优化步骤可大大提高立体视觉定位的精度。但集束调整优化方法对初始值依赖太大，由于远距离特征点或长的运动序列引起的弱结构会造成收敛速度慢或收敛发散等数值稳定性低的缺点。

发明内容

本发明的目的在于克服现有技术中的不足和缺点，提出一种利用视差空间集束调整的高精度实时立体视觉定位方法。相对于集束调整，由于视差空间集束调整采用视差空间特征点投影约束和相对坐标表达，所以其参数优化的数值稳定性高，收敛速度快，对初始值依赖小，鲁棒性高，从而使自主导航车在长距离行进下也能够进行精度实时的定位。为此，本发明采用以下技术方案，其步骤如下：

步骤一、对视频流当前帧的立体图像对进行极线校正，视频流的第一帧选为起点帧；视频流由立体相机的摄像头拍摄形成；

步骤二、提取第一图像的CenSurE特征和U-SURF描述符，所述第一图像为所述立体图像对中的左图像和右图像之一；

步骤三、采用SAD区域匹配方法沿第二图像水平方向极线进行一维搜索得到第一图像CenSurE特征对应的图像坐标，从而得到特征点的3D坐标；所述第二图像为所述立体图像对中的左图像和右图像之另一；所述特征点为摄像头拍摄的同一场景图像关键点，具体反映在立体图像第一图像CenSurE特征和第二图像对应图像坐标；

若当前帧为起点帧，则将当前帧的下一帧作为当前帧，重复步骤一至三的处理步骤；否则，对所述当前帧再进行步骤四和五；

步骤四、对当前帧和前一帧立体图像的特征点集进行一一匹配；

步骤五、采用RANSAC算法和2D-3D位姿估计方法得到匹配内点和拍摄当前帧时的所述摄像头运动参数，包括相对位置、相对姿态；所述相对位置为平移参数t，所述相对姿态为旋转参数R；

步骤六、如步骤五所述的当前帧与起点帧之间的帧数、起点帧和当前帧的总和小于N，将步骤五所述的当前帧的下一帧作为当前帧，对所述当前帧进行步骤一至五的处理，否者，进行步骤七和步骤八的处理；N＝2*n+1，n为大于等于1的自然数；

步骤七、利用当前帧前的N帧中的两两相邻帧的匹配内点之间的视差空间投影约束，采用视差空间集束调整对拍摄当前帧的前n帧摄像头的运动参数进行迭代优化；

如步骤七所述的当前帧与起点帧之间的帧数等于N，则所述迭代优化的初值为当前帧前的n帧的摄像头运动参数；否则，所述迭代优化的初值为当前帧前的n-1帧在视差空间集束调整后的摄像头运动参数和当前帧运动参数；

步骤八、利用拍摄所述当前帧前第1帧时摄像头的全局位置姿态参数和拍摄所述当前帧时摄像头运动参数的迭代优化值，得到拍摄当前帧时摄像头的全局位置姿态参数作为拍摄实时帧时摄像头的全局位置姿态参数，以对摄像头实时立体视觉定位。

在采用上述技术方案的基础上，本发明还可采用以下进一步的技术方案：

在所述步骤一中，采用以下步骤：

利用已标定立体相机的参数，采用极线校正方法对立体图像对进行校正，使立体图像对的左右图像对应的特征点位于同一水平扫描线上，同时得到校正后立体相机基线长度B和焦距f。

在所述步骤二中，采用以下步骤：

(1)、计算得到第一图像的积分图像；

(2)、利用积分图像在每个像素上计算六个尺度的盒子核近似高斯拉普拉斯滤波器响应；

(3)、采用非极大值抑制检测局部极大值得到CenSurE特征，所述CenSurE特征包括其图像坐标(u，v)、尺度s及极大值；

(4)、利用积分图像和CenSurE特征的图像坐标(u，v)和尺度s计算对应的U-SURF描述符。

在所述步骤三中，采用以下步骤：

对第一图像的每个CenSurE特征在第二图像采用简单的SAD立体匹配方法沿水平方向极线进行一维搜索匹配，从而得到第一图像CenSurE特征在第二图像上的对应位置坐标(u₂，v)，则得到整数视差d＝u₂-u；采用如下二次函数对视差值进行拟合，从而达到亚象素级视差：

$d_{\mathrm{subpixel}}=d+\frac{C_{d-1}-C_{d+1}}{2(C_{d-1}-{2C}_d+C_{d+1})}---\left(1\right)$

C_d为求得的SAD匹配代价， $\mathrm{\ω}=\left(\begin{array}{c}u\\ v\\ d\end{array}\right)$ 表示视差空间特征点，由u、v、d可以计算得到特征点的3D坐标X。

在所述步骤四中，采用以下步骤进行匹配：

取当前帧第一图像中的某个特征点U-SURF描述符，与前一帧第一图像中在某个视差范围(-L，L)内的特征点U-SURF描述符计算二者之间的欧式距离，如果最近的欧式距离与次近的欧式距离的比值少于某个比例阀值并且最近邻的距离小于某个阀值时则接受这一对特征点为匹配点，否则拒绝；在匹配过程中只对极大值同为正值或同为负值的CenSurE特征的U-SURF描述符进行匹配；

重复以上步骤，得到至少3对匹配点。

在所述步骤五中，采用以下步骤：

(1)、在步骤四得到的匹配点集中随机采样3对匹配点；

(2)、利用2D-3D位姿估计算法和3对匹配点计算拍摄当前帧时摄像头的运动参数R和t；

(3)、利用该运动参数R和t计算所有匹配点在立体相机上的再投影误差：

|m_l-K[RX+t]|＜τ，|m_r-K[RX+t+(B 0 0)^T]|＜τ        (2)

其中m_l＝(u，v)^T和m_r＝(u+d，v)^T为当前帧特征点在第一和第二图像中的坐标，K为为摄像头内部参数矩阵；满足(2)式的匹配点则为匹配内点；其中τ为一固定阀值，X为前一帧特征点3D坐标；

(4)、多次重复步骤(1)、(2)、(3)，选取匹配内点数最多的计算结果作为最后的匹配内点和拍摄当前帧时的摄像头初始运动参数R和t。

在所述步骤七中，采用以下步骤：

(1)、把旋转矩阵R转化为四元数(q₁，q₂，q₃，q₄)，其中则q₁，q₂，q₃为独立变量，加上平移参数t＝[x y z]^T，则相邻二帧之间的运动参数θ_t＝[q₁ q₂ q₃ x y z]^T为6维向量；

(2)、利用相邻二帧之间的匹配内点得到最近N帧图像任意帧之间的匹配点，利用P_k，j表示帧j到帧k之间的匹配点数；

(3)、通过拍摄相邻两帧的摄像头之间的的运动参数级联表达得到拍摄任意非相邻帧摄像头之间的运动参数；相邻二帧摄像头之间的运动参数即为拍摄各帧时的摄像头的运动参数；

(4)、采用增量式局部参数更新优化方式对运动参数进行优化，本次优化得到的参数作为下一轮优化的初始值；在当前帧t，仅优化其之前的n帧的运动参数，但包含其之前的N帧的视差空间特征点投影约束，设局部视差空间集束调整的目标函数为F(θ)，待优化的运动参数为θ＝{θ_t-n+1，...θ_t}，则目标函数表达如下：

$F\left(\mathrm{\θ}\right)=\underset{j=t-N}{\overset{t-1}{\mathrm{\Σ}}}\underset{k=t-n+1,k>j}{\overset{t}{\mathrm{\Σ}}}\underset{i=0}{\overset{P_{k,j}-1}{\mathrm{\Σ}}}(f_{k,j,i}\left(\mathrm{\θ}\right)-{\widetilde{\mathrm{\ω}}}_{k,i}){\mathrm{\Σ}}^{-1}{(f_{k,j,i}\left(\mathrm{\θ}\right)-{\widetilde{\mathrm{\ω}}}_{k,i})}^T---\left(3\right)$

表示特征点i在摄像机帧k上的视差空间真实测量坐标，f_k，j，i(θ^t)表示特征点i在帧j向帧k运动后的再投影视差空间坐标。∑为视差空间特征点的协方差矩阵；

(5)、采用Levenberg-Marquard算法对目标函数F(θ)中的运动参数θ＝{θ_t-n+1，...θ_t}进行非线性迭代优化。

在所述步骤八中，采用以下步骤：

(1)、利用拍摄当前帧前第n+1帧时摄像头相对于世界坐标系的旋转参数R和平移参数t、拍摄当前帧前第n帧时摄像头的运动参数θ_t-n+1计算得到拍摄当前帧前第n帧时摄像头相对于世界坐标系的旋转参数和平移参数；

(2)、利用拍摄当前帧前第n+1帧时摄像头相对于世界坐标系的各个旋转参数R和平移参数t、拍摄当前帧前n帧的摄像头运动参数θ＝{θ_t-n+1，...θ_t}计算得到拍摄当前帧时摄像头相对于世界坐标系的旋转参数R和平移参数t；拍摄当前帧时摄像头相对于世界坐标系的旋转参数和平移参数即为立体相机6个自由度的3D定位信息。

由于采用本发明的技术方案，本发明具有的有益效果：(1)、仅利用立体相机得到的视频流进行移动机器人的导航定位，具有完全的自主性。(2)、利用U-SURF描述符进行特征匹配，对运动过程中出现的光照变化、运动模糊、大的旋转和尺度变换具有较强的鲁棒性。(3)、利用视差空间集束调整优化方法使自主导航车在长距离行进下也能够进行精度实时的定位，其具有对优化初始值依赖程度低，收敛速度快，数值稳定性高等优点，与利用集束调整的立体视觉定位方法相比，定位精度更高，速度更快。(4)、整个立体视觉定位方法计算速度快，可在线实时运行。

附图说明

图1是本发明的总体流程图；

图2是当前帧的第一和第二图像立体匹配图；

图3是当前帧与前一帧的特征匹配图；

图4是演示特征点多帧匹配约束的示意图；

图5是演示多帧运动约束的示意图；

图6是演示图2所示视差空间投影函数的雅克比矩阵转置JT的示意图；

图7是增量式调整示意图。

图8是立体定位结果对比图。

具体实施方式：

为了使本发明的技术方案和优点更加清楚，下面结合附图对本发明做进一步的详细描述。本发明的总体流程图如图1所示。按照该流程图所述，步骤如下：

1、立体图像校正：

视频流的第一帧选为起点帧；视频流由立体相机的摄像头拍摄形成；利用已标定立体相机的参数(内参，外参，畸变参数)，采用极线校正方法对立体图像对进行校正，使左右图像对应的特征点位于同一水平扫描线上，同时得到校正后立体相机基线长度B和焦距f。极线校正方法采用Bouguet算法。

2、第一图像CenSurE特征和U-SURF描述符提取：

CenSurE特征计算过程如下：

(1)、计算得到第一图像的积分图像；

(2)、利用积分图像在每个像素上计算六个尺度的盒子核近似高斯拉普拉斯滤波器响应；

(3)、采用非极大值抑制检测局部极大值得到CenSurE特征，所述CenSurE特征包括其位置(u，v)、尺度以及极大值；

U-SURF描述符计算过程如下：

(1)、以CenSurE特征点为中心取20s×20s的邻域，将该窗口区域分成4×4的子区域。

(2)、在每个子区域内，计算5s×5s范围内的x和y方向的哈尔小波响应d_x、d_y，并赋予响应值以高斯权重系数，采样步长为s。

(3)、将每个子区域内的响应值和响应绝对值相加，并用四维描述向量V＝(∑d_x，∑d_y，∑|d_x|，∑|d_y|)表示子区域，所以，每个特征点的描述符就是一个64维向量；

(4)、将描述符向量归一化，去除光照影响。

3、第一和第二图像立体匹配：

对第一图像的每个CenSurE特征在第二图像采用简单的SAD立体匹配方法沿水平方向极线进行一维搜索匹配，从而得到第一图像CenSurE特征在第二图像上的对应图像坐标(u₂，v)，则得到整数视差d＝u₂-u。采用如下二次函数对视差值进行拟合，从而达到亚象素级视差：

$d_{\mathrm{subpixel}}=d+\frac{C_{d-1}-C_{d+1}}{2(C_{d-1}-{2C}_d+C_{d+1})}---\left(1\right)$

C_d为求得的SAD匹配代价。 $\mathrm{\ω}=\left(\begin{array}{c}u\\ v\\ d\end{array}\right)$ 表示视差空间特征点。，由u、v、d可以计算得到特征点的3D坐标X。图2给出了当前帧的第一和第二图像立体匹配图。

若当前帧为起点帧，则将当前帧的下一帧作为当前帧，重复步骤1至3的处理步骤；否则，对所述当前帧再进行步骤4和5；

4、与前一帧的特征匹配：

取当前帧第一图像中的某个特征点U-SURF描述符，与前一帧第一图像中在某个视差范围(-L，L)内的特征点U-SURF描述符计算二者之间的欧式距离，如果最近的欧式距离与次近的欧式距离的比值少于某个比例阀值并且最近邻的距离小于某个阀值时则接受这一对特征点为匹配点，否则拒绝；在匹配过程中只对极大值同为正值或同为负值的CenSurE特征的U-SURF描述符进行匹配。图3给出了当前帧与前一帧的特征匹配图。

5、计算匹配内点和初始运动参数：

(1)、在上述步骤得到的匹配点集中随机采样3对匹配点；

(2)、利用2D-3D位姿估计算法和3对匹配点计算拍摄当前帧时摄像头的运动参数R和t；

(3)、利用该运动参数R和t计算所有匹配点在立体相机上的再投影误差：|m_l-K[RX+t]|＜τ，|m_r-K[RX+t+(B 0 0)^T]|＜τ        (2)

其中m_l＝(u，v)^T和m_r＝(u+d，v)^T为当前帧特征点在第一和第二图像中的坐标，K为为摄像头内部参数矩阵；满足(2)式的匹配点则为匹配内点；其中τ为一固定阀值，X为前一帧特征点3D坐标；

(4)、多次重复步骤(1)、(2)、(3)，选取匹配内点数最多的计算结果作为最后的匹配内点和拍摄当前帧时的摄像头的初始运动参数R和t。

6、如步骤5所述的当前帧与起点帧之间的帧数(包含起点帧和当前帧)小于N，将步骤5所述的当前帧的下一帧作为当前帧，对所述当前帧进行步骤一至五的处理，否则，进行步骤六和步骤七的处理；N＝2*n+1，n为大于等于1的自然数；

7、视差空间集束调整：

(1)、建立视差空间投影函数：

设 $W=\left(\begin{array}{c}\mathrm{\ω}\\ 1\end{array}\right),$ 表示视差空间特征点的齐次坐标。设 ${\mathrm{\ω}}^{\′}=\left(\begin{array}{c}{u}^{\′}\\ v^{\′}\\ d^{\′}\end{array}\right)$ 为运动后视差空间特征点，则存在下列视差空间投影函数：

$u^{\′}=\frac{h_1^{T}W}{h_4^{T}W},$ $v^{\′}=\frac{h_2^{T}W}{h_4^{T}W},$ $d^{\′}=\frac{h_3^{T}W}{h_4^{T}W}---\left(3\right)$

其中 $H(R,t)=\left(\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 0& \mathrm{fB}\\ 0& 0& 1& 0\end{array}\right)\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right){\left(\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 0& \mathrm{fB}\\ 0& 0& 1& 0\end{array}\right)}^{-1}=\left(\begin{array}{c}{h}_1^T\\ h_2^T\\ h_3^T\\ h_4^T\end{array}\right)---\left(4\right)$

(2)、运动参数转换：

把旋转矩阵R转化为四元数(q₁，q₂，q₃，q₄)，其中

则q₁，q₂，q₃为独立变量，加上平移参数t＝[x y z]^T，则相邻二帧之间的运动参数θ_t＝[q₁ q₂ q₃ x y z]^T为6维向量；

(3)、采用相对坐标表达任意帧之间的运动参数：

在特征点匹配时，有许多特征点会同时被两帧以上的立体相机观测到，产生特征点多帧匹配约束，如图2所示。相邻二帧之间的运动参数级联表达得到任意非相邻帧之间的运动参数；设 $G_{j,j+1}=\left(\begin{array}{cc}{R}_{j,j+1}& t_{j,j+1}\\ 0& 1\end{array}\right)$ 为4×4的齐次坐标矩阵，表示时间j相邻帧之间的运动参数矩阵，则摄像机帧k相对于帧j的运动参数矩阵为：

G_j，k＝[G_j，j+1G_j+1，j+2...G_k-1，k]^-1                                    (5)

则H_j，k(R，t)＝ΓG_j，kΓ^-1                                             (6)

(6)式中 $\mathrm{\Γ}=\left(\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 0& \mathrm{fB}\\ 0& 0& 1& 0\end{array}\right)$

由于非相邻帧之间也存在匹配点，从而产生了多帧运动约束，如图3所示。除了连续相邻二帧之间存在运动约束，非相邻帧之间也存在运动约束。图3中，相邻帧之间的运动约束如实线所示，非相邻之间的运动约束如虚线所示。这种匹配点产生的运动约束，可以通过特征点视差空间投影约束联系。

(4)、建立增量式视差空间集束调整的参数优化方程：

采用增量式局部参数更新优化方式对运动参数进行优化，本次优化得到的参数作为下一轮优化的初始值。在当前帧t，仅优化最后n帧的运动参数，但包含最后N帧(N＝2*n+1)的视差空间特征点投影约束，图4给出了增量式调整示意图。设局部视差空间集束调整的目标函数为F(θ)，待优化的运动参数为θ＝{θ_t-n+1，...θ_t}，则目标函数表达如下：

$F\left(\mathrm{\θ}\right)=\underset{j=t-N}{\overset{t-1}{\mathrm{\Σ}}}\underset{k=t-n+1,k>j}{\overset{t}{\mathrm{\Σ}}}\underset{i=0}{\overset{P_{k,j}-1}{\mathrm{\Σ}}}(f_{k,j,i}\left(\mathrm{\θ}\right)-{\widetilde{\mathrm{\ω}}}_{k,i}){\mathrm{\Σ}}^{-1}{(f_{k,j,i}\left(\mathrm{\θ}\right)-{\widetilde{\mathrm{\ω}}}_{k,i})}^T---\left(7\right)$

表示特征点i在摄像机帧k上的视差空间真实测量坐标。f_k，j，i(θ)表示特征点i在帧k向帧j运动后的再投影视差空间坐标，如下式：

f_k，j，i(θ)＝SPproj[H_j，kW_j，i]＝SPproj[W_k，j，i(θ)]                    (8)

SPproj[]为式(3)的视差空间投影函数。

∑为视差空间特征点的协方差矩阵，如下式：

${\mathrm{\Σ}}_i=\left[\begin{array}{ccc}\mathrm{\δ}& 0& 0\\ 0& \mathrm{\δ}& 0\\ 0& 0& {\mathrm{\δ}}_d\end{array}\right]---\left(9\right)$

δ为图像坐标u、v的噪声方差。δ_d为视差d的噪声方差。δ取值为1，δ_d取值为0.5。

(5)、基于Levenberg-Marquard算法的运动参数优化：

设

$\widetilde{\mathrm{\ω}}={\left[\begin{array}{cccc}{\widetilde{\mathrm{\ω}}}_{t-N+\mathrm{1,0}}& {\widetilde{\mathrm{\ω}}}_{t-N+1,P_{t-N+1,t-N}}& \·\·\·& {\widetilde{\mathrm{\ω}}}_{t,P_{t,t-1}}\end{array}\right]}^T,$

则式(7)为： $F\left(\mathrm{\θ}\right)={(f\left(\mathrm{\θ}\right)-\widetilde{\mathrm{\ω}})}^T{\mathrm{\Σ}}^{-1}(f\left(\mathrm{\θ}\right)-\widetilde{\mathrm{\ω}})---\left(10\right)$

采用Levenberg-Marquard算法对目标函数F(θ)中的运动参数θ＝{θ_t-n+1，...θ_t}进行非线性迭代优化。Levenberg-Marquard算法迭代过程如下：

1)、给定初始值θ。如步骤六所述的当前帧与起点帧之间的帧数等于N，则所述迭代优化的初值为当前帧的前n帧运动参数；否则，所述迭代优化的初值为拍摄前一帧视差空间集束调整后的前n-1帧运动参数和当前帧运动参数；

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

J为f(θ)的雅克比矩阵：

$J=\frac{\∂f\left(\mathrm{\θ}\right)}{\∂\mathrm{\θ}}==\left(\begin{array}{ccc}\frac{{\∂f}_{t-N+1,t-N,0}\left(\mathrm{\θ}\right)}{{\∂\mathrm{\θ}}_{t-n+1}}& ...& \frac{{\∂f}_{t-N+1,t-N,0}\left(\mathrm{\θ}\right)}{\∂{\mathrm{\θ}}_t}\\ ...& ...& ...\\ \frac{{\∂f}_{t,t-1,P_{t,t-1}}\left(x\right)}{\∂{\mathrm{\θ}}_{t-n+1}}& ...& \frac{{\∂f}_{t,t-1,P_{t,t-1}}\left(x\right)}{\∂{\mathrm{\θ}}_t}\end{array}\right)---\left(11\right)$

$\frac{{\∂f}_{k,j,i}\left(\mathrm{\θ}\right)}{\∂{\mathrm{\θ}}_c}=\frac{\∂\mathrm{\Φ}}{{\∂W}_{k,j,i}}\frac{{\∂W}_{k,j,i}}{\∂{\mathrm{\θ}}_c},$ $\frac{\∂W_{k,j,i}}{\∂{\mathrm{\θ}}_c}=\mathrm{\Γ}{G}_{k,c}\frac{{\∂G}_{c,c+1}}{\∂{\mathrm{\θ}}_c}{G}_{c+1,j}{\mathrm{\Γ}}^{-1}{W}_{j,i}---\left(12\right)$

$\frac{\∂G_{c,c+1}}{\∂{\mathrm{\θ}}_c}$ 为4×4×6的张量。

如图5给出了对应于图2所示视差空间投影函数的雅克比矩阵转置(J^T)。

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

4)、若

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

5)、若

停止计算，输出结果。否则置λ＝λ/β，θ＝θ_new，转2)。参数λ、β、ε分别取0.01、10和0.000001。

8、计算当前帧的3D定位信息：

(1)、利用拍摄当前帧前第n+1帧时摄像头相对于世界坐标系的旋转参数R和平移参数t、拍摄当前帧前第n帧时摄像头的运动参数θ_t-n+1计算得到拍摄当前帧前第n帧时摄像头相对于世界坐标系的旋转参数和平移参数；

(2)、利用拍摄当前帧前第n+1帧时摄像头相对于世界坐标系的各个旋转参数R和平移参数t、拍摄当前帧前n帧的摄像头运动参数θ＝{θ_t-n+1，...θ_t}计算得到拍摄当前帧时摄像头相对于世界坐标系的旋转参数R和平移参数t；拍摄当前帧时摄像头相对于世界坐标系的旋转参数和平移参数即为立体相机6个自由度的3D定位信息。

图8给出了立体视觉定位结果对比图，可以看出：利用视差空间集束调整的立体视觉定位方法比利用集束调整的立体视觉定位方法更接近基准路径。

Claims (8)

1.一种利用视差空间集束调整的高精度实时立体视觉定位方法，其特征在于它包括以下步骤：

步骤一、对视频流当前帧的立体图像对进行极线校正，视频流的第一帧选为起点帧；视频流由立体相机的摄像头拍摄形成；

步骤二、提取第一图像的CenSurE特征和U-SURF描述符，所述第一图像为所述立体图像对中的左图像和右图像之一；

步骤三、采用SAD区域匹配方法沿第二图像水平方向极线进行一维搜索得到第一图像CenSurE特征对应的图像坐标，从而得到特征点的3D坐标；所述第二图像为所述立体图像对中的左图像和右图像之另一；所述特征点为摄像头拍摄的同一场景图像关键点，具体反映在立体图像第一图像CenSurE特征和第二图像对应图像坐标；

若当前帧为起点帧，则将当前帧的下一帧作为当前帧，重复步骤一至三的处理步骤；否则，对所述当前帧再进行步骤四和五；

步骤四、对当前帧和前一帧立体图像的特征点集进行一一匹配；

步骤五、采用RANSAC算法和2D-3D位姿估计方法得到匹配内点和拍摄当前帧时的所述摄像头运动参数，包括相对位置、相对姿态；所述相对位置为平移参数t，所述相对姿态为旋转参数R；

步骤六、如步骤五所述的当前帧与起点帧之间的帧数加上起点帧以及当前帧后的帧数总和小于N，将步骤五所述的当前帧的下一帧作为当前帧，对所述当前帧进行步骤一至五的处理，否者，进行步骤七和步骤八的处理；N=2*n+1，n为大于等于1的自然数；

步骤七、利用当前帧前的N帧中的两两相邻帧的匹配内点之间的视差空间投影函数约束，采用视差空间集束调整对拍摄当前帧的前n帧摄像头的运动参数进行迭代优化；

如步骤七所述的当前帧与起点帧之间的帧数等于N，则所述迭代优化的初值为当前帧前的n帧的摄像头运动参数；否则，所述迭代优化的初值为当前帧前的n-1帧在视差空间集束调整后的摄像头运动参数和当前帧运动参数；

步骤八、利用拍摄所述当前帧前第1帧时摄像头的全局位置姿态参数和拍摄所述当前帧时摄像头运动参数的迭代优化值，得到拍摄当前帧时摄像头的全局位置姿态参数作为拍摄实时帧时摄像头的全局位置姿态参数，以对摄像头实时立体视觉定位。

2.根据权利要求1所述的利用视差空间集束调整的高精度实时立体视觉定位方法，其特征在于在所述步骤一中，采用以下步骤：

利用已标定立体相机的参数，采用极线校正方法对立体图像对进行校正，使立体图像对的左右图像对应的特征点位于同一水平扫描线上，同时得到校正后立体相机基线长度B和焦距f。

3.根据权利要求1所述的利用视差空间集束调整的高精度实时立体视觉定位方法，其特征在于在所述步骤二中，采用以下步骤：

（1）、计算得到第一图像的积分图像；

（2）、利用积分图像在每个像素上计算六个尺度的盒子核近似高斯拉普拉斯滤波器响应；

（3）、采用非极大值抑制检测局部极大值得到CenSurE特征，所述CenSurE特征包括其图像坐标(u,v)、尺度s及极大值；

（4）、利用积分图像和CenSurE特征的图像坐标(u,v)和尺度s计算对应的U-SURF描述符。

4.根据权利要求1或3所述的利用视差空间集束调整的高精度实时立体视觉定位方法，其特征在于在所述步骤三中，采用以下步骤：

对第一图像的每个CenSurE特征在第二图像采用简单的SAD立体匹配方法沿水平方向极线进行一维搜索匹配，从而得到第一图像CenSurE特征在第二图像上的对应位置坐标(u₂,v)，则得到整数视差d=u₂-u；采用如下二次函数对视差值进行拟合，从而达到亚象素级视差：

$d_{\mathrm{subpixel}}=d+\frac{C_{d-1}-C_{d+1}}{2(C_{d-1}-2C_d+C_{d+1})}---\left(1\right)$

C_d为求得的SAD匹配代价， $\mathrm{\ω}=\left(\begin{array}{c}u\\ v\\ d\end{array}\right)$ 表示视差空间特征点，由u、v、d可以计算得到特征点的3D坐标X。

5.根据权利要求1或3所述的利用视差空间集束调整的高精度实时立体视觉定位方法，其特征在于在所述步骤四中，采用以下步骤进行匹配：

取当前帧第一图像中的某个特征点U-SURF描述符，与前一帧第一图像中在某个视差范围(-L，L)内的特征点U-SURF描述符计算二者之间的欧式距离，如果最近的欧式距离与次近的欧式距离的比值少于某个比例阀值并且最近邻的距离小于某个阀值时则接受这一对特征点为匹配点，否则拒绝；在匹配过程中只对极大值同为正值或同为负值的CenSurE特征的U-SURF描述符进行匹配；

重复以上步骤，得到至少3对匹配点。

6.根据权利要求5所述的利用视差空间集束调整的高精度实时立体视觉定位方法，其特征在于在所述步骤五中，采用以下步骤：

（1）、在步骤四得到的匹配点集中随机采样3对匹配点；

（2）、利用2D-3D位姿估计算法和3对匹配点计算拍摄当前帧时摄像头的运动参数R和t；

（3）、利用该运动参数R和t计算所有匹配点在立体相机上的再投影误差：

|m_l-K[RX+t]|＜τ,|m_r-K[RX+t+(B 0 0)^T]|＜τ       (2)

其中m_l＝（u，v）^T和m_r＝（u+d，v）^T为当前帧特征点在第一和第二图像中的坐标，K为为摄像头内部参数矩阵；满足(2)式的匹配点则为匹配内点；其中τ为一固定阀值，X为前一帧特征点3D坐标；

（4）、多次重复步骤（1）、（2）、（3），选取匹配内点数最多的计算结果作为最后的匹配内点和拍摄当前帧时的摄像头初始运动参数R和t。

7.根据权利要求1所述的利用视差空间集束调整的高精度实时立体视觉定位方法，其特征在于在所述步骤七中，采用以下步骤：

(1)、把旋转矩阵R转化为四元数(q₁,q₂,q₃,q₄)，其中

则q₁,q₂,q₃为独立变量，加上平移参数t＝[x y z]^T，则相邻二帧之间的运动参数θ_t＝[q₁ q₂ q₃ x y z]^T为6维向量；

(2)、利用相邻二帧之间的匹配内点得到最近N帧图像任意帧之间的匹配点，利用P_k,j表示帧j到帧k之间的匹配点数；

(3)、通过拍摄相邻两帧的摄像头之间的的运动参数级联表达得到拍摄任意非相邻帧摄像头之间的运动参数；相邻二帧摄像头之间的运动参数即为拍摄各帧时的摄像头的运动参数；

(4)、采用增量式局部参数更新优化方式对运动参数进行优化，本次优化得到的参数作为下一轮优化的初始值；在当前帧t，仅优化其之前的n帧的运动参数，但包含其之前的N帧的视差空间特征点投影约束，设局部视差空间集束调整的目标函数为F(θ)，待优化的运动参数为θ＝{θ_t-n+1,...θ_t}，则目标函数表达如下：

$\left(\mathrm{\θ}\right)=\underset{j=t-N}{\overset{t-1}{\mathrm{\Σ}}}\underset{k=t-n+1,k>j}{\overset{t}{\mathrm{\Σ}}}\underset{i=0}{\overset{P_{k,j}-1}{\mathrm{\Σ}}}(f_{k,j,i}\left(\mathrm{\θ}\right)-{\widetilde{\mathrm{\ω}}}_{k,i}){\mathrm{\Σ}}^{-1}{(f_{k,j,i}\left(\mathrm{\θ}\right)-{\widetilde{\mathrm{\ω}}}_{k,i})}^T---\left(3\right)$

表示特征点i在摄像机帧k上的视差空间真实测量坐标，f_k,j,i(θ^t)表示特征点i在帧j向帧k运动后的再投影视差空间坐标。∑为视差空间特征点的协方差矩阵；

(5)、采用Levenberg-Marquard算法对目标函数F(θ)中的运动参数θ＝{θ_t-n+1,...θ_t}进行非线性迭代优化。

8.根据权利要求7所述的利用视差空间集束调整的高精度实时立体视觉定位方法，其特征在于在所述步骤八中，采用以下步骤：

(1)、利用拍摄当前帧前第n+1帧时摄像头相对于世界坐标系的旋转参数R和平移参数t、拍摄当前帧前第n帧时摄像头的运动参数θ_t-n+1计算得到拍摄当前帧前第n帧时摄像头相对于世界坐标系的旋转参数和平移参数；

(2)、利用拍摄当前帧前第n+1帧时摄像头相对于世界坐标系的各个旋转参数R和平移参数t、拍摄当前帧前n帧的摄像头运动参数θ＝{θ_t-n+1,...θ_t}计算得到拍摄当前帧时摄像头相对于世界坐标系的旋转参数R和平移参数t；拍摄当前帧时摄像头相对于世界坐标系的旋转参数和平移参数即为立体相机6个自由度的3D定位信息。

Images