[发明专利]一种像机相对位姿估计方法及系统

权利要求书

1.一种像机相对位姿估计方法，其特征在于，包括：

步骤1，利用仿射不变特征描述子在两视图之间建立多个仿射匹配点对；

步骤2，根据运动约束条件构建约束方程，利用单个仿射匹配点求解约束方程闭合形式解获得两视图之间的相对位姿；

步骤3，通过获得的相对位姿结合RANSAC框架剔除仿射匹配点对中的误匹配点对，确定仿射匹配点对内点；

步骤4，利用两视图之间仿射匹配点对的内点优化相对位姿并输出；

所述步骤2中所述运动约束条件为平面运动约束条件或垂直方向已知的空间运动约束条件；

当用于采集视图的像机与惯性测量单元固定连接，像机垂直方向运动已知，所述步骤2包括：

像机的Y轴平行于重力方向，像机的X-Z平面垂直于重力方向；像机坐标系转换到校正后像机坐标系的旋转矩阵R_imu表示为:

式中θ_x和θ_z分别为俯仰角和滚转角；

用和分别表示由惯性测量单元提供的用于校正视图i和j的旋转矩阵，则视图i和j中校正后的图像坐标可以表示为：

原始视图i和j之间的基本矩阵可以写成：

表示校正后视图i和j之间的简化基本矩阵，其中是校正后视图i和j之间的平移方量，R_y是校正后视图i和j之间的旋转矩阵；将公式(19)带入公式

上式根据公式(18)可重新表述为：

其中表示校正后的图像匹配对点和之间的仿射矩阵；

将和表示如下：

将公式(23)带入(22)得到两个等式：

此外，极线约束可以写为：

对于仿射匹配点对(p_i,p_j,A)，方程组(24)～(26)可以表示为Mx＝0，其中x＝[e₁,e₂,e₃,e₄,e₅,e₆]^T是本质矩阵的未知元素向量，M的零空间是三维的，通过三个零空间基向量的线性组合来确定恢复到尺度大小，方程(24),(25),(26)的解：

x＝βm₁+γm₂+m₃,           (27)

其中，根据矩阵M的奇异值分解计算零空间基向量{m_i}_i＝1,2,3，其中β和γ是系数；

要确定β和γ的系数，本质矩阵有两个内部约束，即本质矩阵的奇异性和迹约束：

通过将公式(27)代入公式(28)和(29)，生成具有未知数β和γ的多项式方程组；将方程组转化为关于γ的一元四次方程，进行求解β；一旦获得了系数β和γ，简化的基本矩阵就可以由公式(27)确定，并可通过利用方程(23)分解得到R_y和最后，视图i和j之间的相对姿态可以通过以下公式获得

当像机为平面运动时，所述步骤2包括：

步骤21a，根据两视图之间的对极约束、两视图中图像匹配点对的已知图像坐标、两视图之间的相对旋转和平移关系构建平面运动航偏角和平移方向角的第一关系方程；平面运动航偏角为假设像机的图像平面垂直于地面绕Y轴的旋转角，平移方向角为像机在平面内移动的方向角；

步骤22a，根据仿射匹配点对信息中局部仿射矩阵与描述两视图之间平面运动的基本矩阵之间的关系及两视图之间平面运动航偏角和平移方向角与基本矩阵之间的关系获得平面运动航偏角和平移方向角的第二关系方程和第三关系方程；

步骤23a，通过闭式解方法或最小二乘法对上述方程求解获得两视图之间平面运动航偏角和平移方向角；

所述步骤21a包括：

视图i至j之间的对极约束如下：

其中p_i＝[u_i,v_i,1]^T，p_j＝[u_j,v_j,1]^T分别是视图i和j中图像匹配点对的归一化图像坐标，E＝[t]_×R是基本矩阵，R和t分别代表两视图之间的相对旋转和平移关系；

对于平面运动，假设像机的图像平面垂直于地面，两个视图之间仅存在绕Y轴的旋转角θ和平面内的平移方向角φ，因此从视图i到j的旋转矩阵R＝R_y以及平移向量t可以写成：

其中ρ是视图i和j之间的运动距离，基于公式(2)和(3)可重新构造了平面运动下的基本矩阵E＝[t]_×R_y

通过将方程(4)式代入方程式(1)，对极约束可以写成：

v_isin(θ-φ)+v_iu_jcos(θ-φ)+v_jsin(φ)-u_iv_jcos(φ)＝0.(5)

上述公式(5)即为第一关系方程；

步骤22a包括：

两个视图之间的仿射匹配点对为：(p_i,p_j,A)，局部仿射矩阵A描述了图像匹配点对(p_i,p_j)之间邻域信息的关系，定义如下：

基本矩阵E与局部仿射矩阵A的关系可描述如下：

其中n_i＝E^Tp_j与n_j＝Ep_i分别表示视图i和j中的极线，定义是3×3矩阵：

将公式(4)代入公式(7)，得到了将仿射矩阵与相对位姿相关系的两个方程

a₁₁v_icos(θ-φ)+a₂₁sin(φ)-(a₂₁u_i+v_j)cos(φ)＝0,                   (9)

sin(θ-φ)+(a₁₂v_i+u_j)cos(θ-φ)+a₂₂sin(φ)-a₂₂u_icos(φ)＝0.          (10)

公式(8)、(9)分别为第二、第三关系方程；

当用于采集视图的像机已标定内参，步骤23a中的闭式解方法的步骤包括：

对于仿射匹配点对，公式(5)、(9)和(10)表示为：

Cx＝0,x＝[sin(θ-φ),cos(θ-φ),sin(φ),cos(φ)]^T

用记号表示：

忽略x项之间的隐式约束，即和x应属于零空间C,因此，矩阵C^TC最小特征值对应的特征向量即为系统x的解；

通过SVD获得x，则角度θ和φ分别为：

或，当用于采集视图的像机已标定内参，步骤23a中的最小二乘法的步骤包括：

公式(5)、(9)、(10)的三角函数隐式约束可以重新表述为:

用系数a_i,b_i,c_i和d_i表示公式(5)、(9)及(10)中的问题系数；该方程组有4个未知数和5个独立的约束条件，因此公式(3)为超定方程组；通过下式找到最小二乘解：

采用拉格朗日乘子法求解(14)中的所有极值点；拉格朗日乘子是：

通过令和的偏导数为零,得到一个含有未知数和的方程组；此方程组含有6个未知数{x₁,x₂,x₃,x₄,λ₁,λ₂},且秩为2；可通过格罗布纳基方法求解该方程组；

或，当用于采集视图的像机未标定内参，除焦距之外的内参数已知，所述步骤23a包括：

将主点作为像平面的中心；将原始图像平面i和j中一个点的坐标分别标记为和且g＝f^-1，得到下列关系式

将式(16)代入(5)、(9)、(10)得到三个方程；把式(11)代入这三个方程并与两个三角约束结合起来，得到了如下多项式方程组：

上面的方程组包含5个未知数{x₁,x₂,x₃,x₄,g}，秩为3；通过格罗布纳基方法求解该方程组。

2.一种像机相对位姿估计系统，其特征在于，包括存储器和处理器，所述存储器存储有像机相对位姿估计程序，所述处理器在运行所述像机相对位姿估计程序时执行权利要求1所述方法的步骤。