CN111696158B - 基于仿射匹配点对的多像机系统相对位姿估计方法及装置 - Google Patents

Abstract

本发明公开一种基于仿射匹配点对的多像机系统相对位姿估计方法针对多像机系统在不同预设运动状态下的相对位姿估计问题，提出了对极约束和推导出新的仿射矩阵约束，根据至少一个仿射匹配点表示对应的仿射矩阵约束和对极约束，确定系统方程中待求解未知参量的值。该方法充分利用视图之间的仿射匹配点对信息，减少了相对位姿估计所需的点对数量，在推导出新的仿射矩阵约束条件下最少只需两个仿射匹配点对即可恢复多像机系统的6DOF相对位姿；在平面运动约束下，仅需一个仿射匹配点对；在已知垂直方向的条件，仅需二个仿射匹配点对。因此本发明的方法效率高、精度高，可以有效地用于视觉里程计和三维重建等领域中的误匹配点对的剔除和初始运动估计。

Description

基于仿射匹配点对的多像机系统相对位姿估计方法及装置

技术领域

本发明涉及多像机系统技术领域，尤其是一种基于仿射匹配点对 的多像机系统相对位姿估计方法及装置。

背景技术

数十年来，同步定位和建图(SLAM)、视觉里程计(VO)和 三维重建(SfM)一直是计算机视觉中活跃的研究主题。这些技术已 成功应用于各类场景，例如自动驾驶和视觉导航等领域。单像机或者 多像机系统的前后两视图之间的相对姿态估计是SLAM和SfM系统 的重要组成部分，一直被视为SLAM和SfM系统的基础算法。因此， 提高相对姿态估计算法的精度、效率和鲁棒性仍然是人们重点关注的 问题。

典型的多像机SLAM和SfM系统都包含以下主要步骤：首先，通 过特征匹配算法来建立视图之间的图像匹配点对；然后，采用随机抽 样一致(RANdom SAmple Consensus,RANSAC)等算法剔除图像匹配 点对中的误匹配点对。最后，利用图像匹配点对中的内点求解视图之 间的相对位姿关系。其中误匹配点对剔除对于相对位姿估计算法的 精度和鲁棒性至关重要，同时误匹配点对剔除的效率直接影响SLAM 和SfM系统的实时性能。由于在相同的野值比例条件下，RANSAC 的随机采样次数随着最小配置解所需图像匹配点对数量的增加而成 指数型增长，导致效率较低。同时，目前现有的相对位姿估计算法都 是从一组点对应关系(例如SIFT，SURF等特征描述子)估算位姿参 数，精度不高。

发明内容

本发明提供一种基于仿射匹配点对的多像机系统相对位姿估计 方法及装置，用于克服现有技术中效率较低、精度不高等缺陷。

为实现上述目的，本发明提出一种基于仿射匹配点对的多像机系 统相对位姿估计方法，所述方法包括：

获取预设运动状态下多像机系统的外参数；所述外参数包括旋转 量和平移量；

确定所述多像机系统中连续帧之间的仿射匹配点表示；所述仿射 匹配点表示包括：仿射匹配点对和局部仿射矩阵；

根据所述仿射匹配点对和所述外参数，确定多像机系统对应的多 像机参考坐标系中的归一化齐次图像坐标表示，根据所述归一化齐次 图像坐标表示、旋转量和平移量，确定所述多像机系统中的对极约束；

根据所述外参数，确定所述多像机系统中连续帧之间的转换矩阵， 根据所述转换矩阵，确定连续帧之间的基本矩阵；

根据所述基本矩阵，确定所述基本矩阵和所述局部仿射矩阵的关 系表达式，根据所述关系表达式和所述归一化齐次图像坐标表示，确 定所述局部仿射矩阵与所述对极约束的仿射矩阵约束；

确定多像机系统的系统方程；所述系统方程中包括：待求解未知 参量；

根据至少一个仿射匹配点表示对应的仿射矩阵约束和所述对极 约束，确定所述系统方程中待求解未知参量的值；

根据待求解未知参量的值确定预设运动状态下的相对位姿。

为实现上述目的，本发明还提出基于仿射匹配点对的多像机系统 相对位姿估计装置，所述装置包括：

外参数处理模块，用于获取预设运动状态下多像机系统的外参数； 所述外参数包括旋转量和平移量；

确定所述多像机系统中连续帧之间的仿射匹配点表示；所述仿射 匹配点表示包括：仿射匹配点对和局部仿射矩阵；

约束获取模块，用于根据所述仿射匹配点对和所述外参数，确定 多像机系统对应的多像机参考坐标系中的归一化齐次图像坐标表示， 根据所述归一化齐次图像坐标表示、旋转量和平移量，确定所述多像 机系统中的对极约束；根据所述外参数，确定所述像机机系统中连续 帧之间的转换矩阵，根据所述转换矩阵，确定连续帧之间的基本矩阵； 根据所述基本矩阵，确定所述基本矩阵和所述局部仿射矩阵的关系表 达式，根据所述关系表达式和所述归一化齐次图像坐标表示，确定所 述局部仿射矩阵与所述对极约束的仿射矩阵约束；

系统方程获取模块，用于确定多像机系统的系统方程；所述系统 方程中包括：待求解未知参量；

系统方程求解模块，用于根据至少一个仿射匹配点表示对应的仿 射矩阵约束和所述对极约束，确定所述系统方程中待求解未知参量的 值；

位姿估计模块，用于根据待求解未知参量的值确定预设运动状态 下的相对位姿。

为实现上述目的，本发明还提出一种计算机设备，包括存储器和 处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机 程序时实现上述所述方法的步骤。

为实现上述目的，本发明还提出一种计算机可读存储介质，其上 存储有计算机程序，所述计算机程序被处理器执行时实现上述所述方 法的步骤。

与现有技术相比，本发明的有益效果有：

本发明提供的基于仿射匹配点对的多像机系统相对位姿估计方 法针对多像机系统在不同预设运动状态下的相对位姿估计问题，提出 了对极约束和推导出新的约束条件——仿射矩阵约束，根据至少一个 仿射匹配点表示对应的仿射矩阵约束和所述对极约束，确定所述系统 方程中待求解未知参量的值。本发明的方法充分利用视图之间的仿射匹配点对信息，极大减少了相对位姿估计所需的点对数量，且在推导 出新的约束条件——仿射矩阵约束条件下最少只需两个仿射匹配点 对即可恢复多像机系统的6DOF(自由度)相对位姿，即3DOF旋转和 平移量。本发明的方法在平面运动约束下，仅需一个仿射匹配点对；在已知垂直方向的条件，仅需二个仿射匹配点对，因此本发明的方法 效率高、精度高，可以有效地用于视觉里程计和三维重建等领域中的 误匹配点对的剔除和初始运动估计，在自动驾驶汽车和地面机器人操 作的场景中具有广泛的应用前景。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面 将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而 易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域 普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些 附图示出的结构获得其他的附图。

图1为本发明提供的基于仿射匹配点对的多像机系统相对位姿估 计方法流程图；

图2为本发明实施例中一般运动状态下多像机系统的示意图；

图3为本发明实施例中在平面运动状态下摄像机的运动方向示意 图；

图4为本发明实施例中在已知垂直方向运动状态下摄像机的运动 方向示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做 进一步说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方 案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一 部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域 普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施 例，都属于本发明保护的范围。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必 须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现 相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在 本发明要求的保护范围之内。

本发明提出一种基于仿射匹配点对的多像机系统相对位姿估计 方法，如图1所示，所述方法包括：

101：获取预设运动状态下多像机系统的外参数；所述外参数包 括旋转量和平移量；

预设运动状态表示的是多像机系统中相机的运动状态，一般情况 下，相机可以在空间中进行6自由度的运动(q_x,q_y,q_z,t_x,t_y,t_z)，假设 运动状态为平面运动，则，只需要考虑3个自由度q_y,t_x,t_z。

旋转量和平移量表示摄像机的旋转量和平移量，描述的是像机坐 标系与世界坐标系之间的相对位置、姿态关系。

102：确定多像机系统中连续帧之间的仿射匹配点表示；仿射匹 配点表示包括：仿射匹配点对和局部仿射矩阵；

仿射匹配点对为连续帧图像中特征点的归一化齐次图像坐标；

局部仿射矩阵是2×2线性变换矩阵，与仿射匹配点对相关联。

103：根据仿射匹配点对和外参数，确定多像机系统对应的多像 机参考坐标系中的归一化齐次图像坐标表示，根据归一化齐次图像坐 标表示、旋转量和平移量，确定多像机系统中的对极约束；

104：根据外参数，确定多像机系统中连续帧之间的转换矩阵， 根据转换矩阵，确定连续帧之间的基本矩阵；

105：根据基本矩阵，确定基本矩阵和局部仿射矩阵的关系表达 式，根据关系表达式和归一化齐次图像坐标表示，确定局部仿射矩阵 与对极约束的仿射矩阵约束；

106：确定多像机系统的系统方程；系统方程中包括：待求解未 知参量；

107：根据至少一个仿射匹配点表示对应的仿射矩阵约束和对极 约束，确定系统方程中待求解未知参量的值；

108：根据待求解未知参量的值确定预设运动状态下的相对位姿。

本发明提供的基于仿射匹配点对的多像机系统相对位姿估计方 法针对多像机系统在不同预设运动状态下的相对位姿估计问题，提出 了对极约束和推导出新的约束条件——仿射矩阵约束，根据至少一个 仿射匹配点表示对应的仿射矩阵约束和所述对极约束，确定所述系统 方程中待求解未知参量的值。本发明的方法充分利用视图之间的仿射匹配点对信息，极大减少了相对位姿估计所需的点对数量，且在推导 出新的约束条件——仿射矩阵约束条件下最少只需两个仿射匹配点 对即可恢复多像机系统的6DOF(自由度)相对位姿，即3DOF旋转 和平移量。本发明的方法在平面运动约束下，仅需一个仿射匹配点对；在已知垂直方向的条件，仅需二个仿射匹配点对，因此本发明的方法 效率高、精度高，可以有效地用于视觉里程计和三维重建等领域中的 误匹配点对的剔除和初始运动估计，在自动驾驶汽车和地面机器人操 作的场景中具有广泛的应用前景。

在其中一个实施例中，预设运动状态为一般运动状态，一般运动 状态是指在k和k+1时刻两个多像机参考系之间存在3个旋转量和3 个平移量。

一般运动状态下，多像机系统由若干个摄像机C_i组成，如图2所 示。

本实施例中，对于步骤101，获取预设运动状态下多像机系统的 外参数，包括：

获取预设运动状态下多像机系统的外参数，得到旋转量R和平移 量t分别为：

t＝[t_x t_y t_z]^T (8)

式中，q_x,q_y,q_z,t_x,t_y,t_z表示未知参量。旋转量R和平移量t均为矩阵。

对于步骤102，确定多像机系统中连续帧之间的仿射匹配点表示 为(x_ij,x′_ij,A)；仿射匹配点表示包括：仿射匹配点对(x_ij,x′_ij)和局部仿射 矩阵A，其中，x_ij和x′_ij是特征点j的归一化齐次图像坐标，i和j分别 是摄像机序号和特征点序号；A是2×2的线性变换矩阵，与x_ij和x′_ij相 关联。

对于步骤103，根据仿射匹配点对和外参数，确定多像机系统对 应的多像机参考坐标系中的归一化齐次图像坐标表示，根据归一化齐 次图像坐标表示、旋转量和平移量，确定多像机系统中的对极约束， 包括：

301：根据仿射匹配点对(x_ij,x′_ij)和外参数(R_i,t_i)，确定多像机系统 对应的多像机参考坐标系中的归一化齐次图像坐标表示为：

p_ij＝R_ix_ij, p′_ij＝R_ix′_ij. (1)

式中，P_ij表示连续帧中第一帧图像的归一化齐次图像坐标，P'_ij表示 连续帧中第二帧图像的归一化齐次图像坐标；R_i表示第i个摄像机的 旋转量，即，即(R_x,R_y,R_z)；x_ij表示连续帧中第一帧图像中的仿射匹 配点，x'_ij表示连续帧中第二帧图像中的仿射匹配点，x_ij、x'_ij组成仿 射匹配点对；

观测视线的单位向量(u_ij,u′_ij)在多像机参考坐标系中表示为： u_ij＝p_ij/‖p_ij‖,u′_ij＝p′_ij/‖p′_ij‖。与观测视线对应的6维矢量

线表示 为：

302：根据归一化齐次图像坐标表示、旋转量和平移量，确定多 像机系统中的对极约束为：

式中，I_ij和I'_ij分别表示k和k+1时刻多像机参考坐标系中的

线； t表示平移量；R表示旋转量；[t]_×表示平移量t的反对称矩阵。

对于步骤104，根据外参数，确定多像机系统中连续帧之间的转 换矩阵，根据转换矩阵，确定连续帧之间的基本矩阵，包括：

401：根据外参数，确定多像机系统中连续帧之间的转换矩阵为：

式中，(R_Ci,t_Ci)表示转换矩阵，R_Ci表示相机坐标系C_i在时刻k和k+1两 连续帧之间的旋转量，t_Ci表示相机坐标系C_i在时刻k和k+1两连续帧 之间的平移量；R_i表示第i个摄像机的旋转量；t_i表示第i个摄像机的 平移量，即(t_x,t_y,t_z)；R表示旋转量；t表示平移量；T表示转置；

402：根据转换矩阵，确定连续帧之间的基本矩阵为：

式中，E表示基本矩阵；[R_it_Ci]_×＝R[t_i]_×R^T+[t]_×-[t_i]_×。

对于步骤105，根据基本矩阵，确定基本矩阵和局部仿射矩阵的 关系表达式，根据关系表达式和归一化齐次图像坐标表示，确定局部 仿射矩阵与对极约束的仿射矩阵约束，包括：

501：根据基本矩阵E，确定基本矩阵E和局部仿射矩阵A的关系 表达式为：

式中，n_ij～E^Tx′_ij和n′_ij～Ex_ij，表示相机坐标系C_i在时刻k和k+1两连续 帧对应的极线；()_(1:2)表示方程式系统的第一和第二方程式；

是一 个3×3的矩阵，

A表示局部仿射矩阵。通过将公式(4) 带入公式(19)得到：

式中，符号含义同公式(4)和(19)。

502：根据关系表达式和归一化齐次图像坐标表示，确定局部仿 射矩阵与对极约束的仿射矩阵约束为：

式中，符号含义同公式(1)和(5)。

实际应用中，根据公式(1)，重构并扩展公式(5)，即可得到公式 (6)。

对于步骤106，根据仿射匹配点表示(x_ij,x′_ij,A)，从公式(2)和公式 (6)得到六个未知参量{q_x,q_y,q_z,t_x,t_y,t_z}。将未知参量q_x，q_y，q_z与t_x，t_y， t_z分开，得到一个方程系统为：

式中，M_ij表示在一般状态下系数矩阵M(q_x,q_y,q_z的元素， i＝1～3,j＝1～4；q_x,q_y,q_z,t_x,t_y,t_z表示未知参量。

对于步骤107，根据至少一个仿射匹配点表示对应的仿射矩阵约 束和对极约束，确定系统方程中待求解未知参量的值，包括：

根据两个仿射匹配点表示对应的仿射矩阵约束和对极约束，采用

basis工具确定所述系统方程中待求解6个未知参量的值。

本实施例中，具体过程为：

公式(9)对六个未知参量施加三个独立约束，即x,y,z三个方向上 的约束。因此，需要两个仿射匹配点对(x_ij,x′_ij)来恢复6DOF(自由度) 一般运动状态下多像机系统的相对位姿，从2个仿射匹配点对(x_ij,x′_ij) 中得到了一个由6个独立约束组成的方程组。最终通过

basis工 具从6个独立约束方程组中求解6个未知数{q_x,q_y,q_z,t_x,t_y,t_z}。

对于步骤108，根据待求解未知参量的值确定预设运动状态下的 相对位姿具体为：

将步骤107获得的未知参量q_x,q_y,q_z,t_x,t_y,t_z带入公式(7)和(8)， 即可获得旋转量R和平移量t。

在另一个实施例中，预设运动状态为平面运动状态，平面运动状 态是指在k和k+1时刻两个多像机参考系之间只有Y轴旋转量和2D 平移量，如图3所示，有三个未知参量：偏航角θ，平移方向φ和平 移距离ρ。

本实施例中，对于步骤101，与在一般运动状态下的公式(7)和(8) 相似；

获取预设运动状态下多像机系统的外参数，包括：

获取预设运动状态下多像机系统的外参数，得到旋转量R_y和平 移量

分别为：

式中，q_y,t_x,t_z表示未知参量，

t_x＝ρsin(φ)，t_z＝-ρcos(φ)，ρ 表示连续帧之间相机的平移距离；

对于步骤102～105，参考在一般运动状态下的方案。

对于步骤106，将公式(11)、公式(12)带入到公式(2)和公式(6)， 得到了三个未知参量q_y,t_x和t_z的三个多项式的方程组，将单个仿 射匹配点对(x_ij,x′_ij)的三个约束代入未知参量q_y,t_x和t_z的三个多项 式的方程组中，得到多像机系统的系统方程为：

式中，M_ij表示在平面运动状态下系数矩阵M(q_y)的元素， i＝1～3,j＝1～4；q_y,t_x,t_z表示未知参量。

对于步骤107，根据至少一个仿射匹配点表示对应的仿射矩阵约 束和对极约束，确定系统方程中待求解未知参量的值，包括：

701：判断

的行列式是否为零；

是一个方矩阵，所以仅在

的行列式为零 时才具有非零解。

702：若是，

的扩展式为：

式中，quot(a,b)表示计算a除以b的商；w₀～w₆由仿射匹配点对之间的

线和局部仿射矩阵系数构成；q_y表示未知参量；

703：求解所述扩展式，得到一个最大为4个实根的显式解析解， 根据所述显式解析解确定所述系统方程中待求解未知参量的值，具体 为：

求解所述扩展式，得到一个最大为4个实根的显式解析解，找到 q_y的解后，通过将q_y代入M(q_y)中并通过计算M(q_y)零向量来求解系统 方程，即可求出未知参量t_x和t_z。

对于步骤108，根据待求解未知参量的值确定预设运动状态下的 相对位姿具体为：

将步骤107求解的未知参量q_y、t_x和t_z带入公式(11)、公式(12) 中，即可获得旋转量R_y和平移量

在下一个实施例中，预设运动状态为已知垂直方向运动状态，在 已知垂直方向运动状态下，IMU(惯性测量单元)与多像机系统耦合， 并且IMU与参考框架之间的相对旋转是已知的。IMU为参考系提供 已知的侧倾角θ_r和俯仰角θ_p。因此，参考框架可以与测得的重力方向 对齐，使X-Z平面与地面平行，Y轴与重力方向平行，如图4所示， 有四个未知参量：绕Y轴旋转R_y和3D平移

对于步骤101，获取预设运动状态下多像机系统的外参数，包括：

获取预设运动状态下多像机系统的外参数，得到旋转量R_imu和平 移量

为：

式中，θ_p和θ_r分别表示耦合IMU提供的侧倾角和俯仰角；R_imu、R′_imu分别表示在已知垂直方向运动状态下连续帧的旋转量；t表示在一般 运动状态下的平移量；

对于步骤102～105，将IMU提供的时刻k和k+1两连续帧之间多 像机参考坐标系的旋转量分别用R_imu和R′_imu表示，将两个参考系之间 的相对旋转表示为：

R＝(R′_imu)^TR_yR_imu. (20)

将等式(20)带入等式(2)得到对极约束为：

式中，

表示为时刻k和k+1校正后多像机参考坐标系中的

线。

在已知垂直方向运动状态下，像机坐标系C_i在时刻k和k+1两连 续帧之间的转换矩阵表示为：

式中，R_imu、R′_imu分别表示在已知垂直方向运动状态下连续帧的旋转 量；

表示在已知垂直方向运动状态下的平移量；R_y表示在平面运动 状态下的旋转量。

公式(22)可表示为：

式中，

通过将公式(23)带入公式(22)，得到：

相机坐标系C_i两帧之间的基本矩阵E表示为：

式中，

通过将等式(25)带入等式 (23)得到：

在参考框架中的归一化齐次图像坐标表示为(p_ij,p′_ij)，可以表示为：

根据上面的公式(27)，可以将公式(26)扩展为：

式中，

对于步骤106，根据公式(21)和公式(28)，得到四个未知参量q_y,

和

但是一个仿射匹配点对只有三个独立约束。因此需要从 第二个仿射匹配点对中得到更多的约束条件，原则上，可以从公式(21) 和公式(28)中选择一个任意方程式，例如，第一个仿射匹配点对的三 个约束和第二个仿射匹配点对的第一个约束得到4个未知参量中的4 个系统方程为：

式中，

表示在已知垂直方向运动状态下系数矩阵

的元素， i＝1～4,j＝1～4；q_y,

表示未知参量；

对于步骤107，根据至少一个仿射匹配点表示对应的仿射矩阵约 束和对极约束，确定系统方程中待求解未知参量的值，包括：

判断

的行列式是否为零；

是一个方形矩 阵，所以当

的行列式为零时，公式(17)具有非零解。

若是，

的扩展式为六次一元多项式：

式中，quot(a,b)表示计算a除以b的商；

由仿射匹配点对之间的

线和局部仿射矩阵系数构成；q_y表示未知参量；

求解所述扩展式，得到一个最多有6个实根的闭式解，根据所述 闭式解确定所述系统方程中待求解未知参量的值，具体为：

利用伴随矩阵法或Sturm-bracketing法求解所述扩展式，得到一 个最多有6个实根的闭式解，得到q_y。

对于步骤108，根据待求解未知参量的值确定预设运动状态下的 相对位姿具体为：

将步骤107获得的q_y带入公式(11)即可求得在平面运动状态下 的旋转矩阵Ry，再通过公式(20)可以得到在一般状态下的旋转量R， 平移量可以通过

计算得到。

在已知垂直方向运动状态下，仅使用了来自第二仿射匹配点对中 三个独立方程式之一。尽管在RANSAC循环中需要采样两个仿射匹 配点对，但第二个仿射匹配点对提供的剩余两个约束方程用于进行一 致性检查，即利用第二仿射匹配点对的两个剩余方程式直接去除不合 理的解。与常规的两点方法(例如Ackermann运动模型假设的求解 方法)相比，本发明的方法可以在不依赖其他仿射匹配点对的情况下 直接检测到不合理的解，因此这种方法具有显著的计算优势。

本发明还提出一种基于仿射匹配点对的多像机系统相对位姿估 计装置，所述装置包括：

外参数处理模块，用于获取预设运动状态下多像机系统的外参数； 所述外参数包括旋转量和平移量；

确定所述多像机系统中连续帧之间的仿射匹配点表示；所述仿射 匹配点表示包括：仿射匹配点对和局部仿射矩阵；

约束获取模块，用于根据所述仿射匹配点对和所述外参数，确定 多像机系统对应的多像机参考坐标系中的归一化齐次图像坐标表示， 根据所述归一化齐次图像坐标表示、旋转量和平移量，确定所述多像 机系统中的对极约束；根据所述外参数，确定所述像机机系统中连续 帧之间的转换矩阵，根据所述转换矩阵，确定连续帧之间的基本矩阵； 根据所述基本矩阵，确定所述基本矩阵和所述局部仿射矩阵的关系表 达式，根据所述关系表达式和所述归一化齐次图像坐标表示，确定所 述局部仿射矩阵与所述对极约束的仿射矩阵约束；

系统方程获取模块，用于确定多像机系统的系统方程；所述系统 方程中包括：待求解未知参量；

系统方程求解模块，用于根据至少一个仿射匹配点表示对应的仿 射矩阵约束和所述对极约束，确定所述系统方程中待求解未知参量的 值；

位姿估计模块，用于根据待求解未知参量的值确定预设运动状态 下的相对位姿。

本发明还提出一种计算机设备，包括存储器和处理器，所述存储 器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所 述方法的步骤。

本发明还提出一种计算机可读存储介质，其上存储有计算机程序， 所述计算机程序被处理器执行时实现上述所述方法的步骤。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利 范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所 作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括 在本发明的专利保护范围内。

Claims (10)

1.一种基于仿射匹配点对的多像机系统相对位姿估计方法，其特征在于，所述方法包括：

获取预设运动状态下多像机系统的外参数；所述外参数包括旋转量和平移量；

确定所述多像机系统中连续帧之间的仿射匹配点表示；所述仿射匹配点表示包括：仿射匹配点对和局部仿射矩阵；

根据所述仿射匹配点对和所述外参数，确定多像机系统对应的多像机参考坐标系中的归一化齐次图像坐标表示，根据所述归一化齐次图像坐标表示、旋转量和平移量，确定所述多像机系统中的对极约束；

根据所述外参数，确定所述多像机系统中连续帧之间的转换矩阵，根据所述转换矩阵，确定连续帧之间的基本矩阵；

根据所述基本矩阵，确定所述基本矩阵和所述局部仿射矩阵的关系表达式，根据所述关系表达式和所述归一化齐次图像坐标表示，确定所述局部仿射矩阵与所述对极约束的仿射矩阵约束；

确定多像机系统的系统方程；所述系统方程中包括：待求解未知参量；

根据至少一个仿射匹配点对表示对应的仿射矩阵约束和所述对极约束，确定所述系统方程中待求解未知参量的值；

根据待求解未知参量的值确定预设运动状态下的相对位姿。

2.根据权利要求1所述的方法，其特征在于，根据所述仿射匹配点对和所述外参数，确定多像机系统对应的多像机参考坐标系中的归一化齐次图像坐标表示，根据所述归一化齐次图像坐标表示、旋转量和平移量，确定所述多像机系统中的对极约束，包括：

根据所述仿射匹配点对和所述外参数，确定多像机系统对应的多像机参考坐标系中的归一化齐次图像坐标表示为：

p_ij＝R_ix_ij,p’_ij＝R_ix’_ij. (1)

式中，P_ij表示连续帧中第一帧图像的归一化齐次图像坐标，P'_ij表示连续帧中第二帧图像的归一化齐次图像坐标；R_i表示第i个摄像机的旋转量；x_ij表示连续帧中第一帧图像中的仿射匹配点，x'_ij表示连续帧中第二帧图像中的仿射匹配点，x_ij、x'_ij组成仿射匹配点对；

根据所述归一化齐次图像坐标表示、旋转量和平移量，确定所述多像机系统中的对极约束为：

式中，I_ij和I'_ij分别表示k和k+1时刻多像机参考坐标系中的Plücker线；t表示平移量；R表示旋转量；[t]_×表示平移量t的反对称矩阵。

3.根据权利要求1所述的方法，其特征在于，根据所述外参数，确定所述多像机系统中连续帧之间的转换矩阵，根据所述转换矩阵，确定连续帧之间的基本矩阵，包括：

根据所述外参数，确定所述多像机系统中连续帧之间的转换矩阵为：

式中，(R_Ci,t_Ci)表示转换矩阵，R_Ci表示相机坐标系C_i在时刻k和k+1两连续帧之间的旋转量，t_Ci表示相机坐标系C_i在时刻k和k+1两连续帧之间的平移量；R_i表示第i个摄像机的旋转量；t_i表示第i个摄像机的平移量；R表示旋转量；t表示平移量；T表示转置；

根据所述转换矩阵，确定连续帧之间的基本矩阵为：

式中，E表示基本矩阵；[R_it_Ci]_×＝R[t_i]_×R^T+[t]_×-[t_i]_×。

4.根据权利要求1所述的方法，其特征在于，根据所述基本矩阵，确定所述基本矩阵和所述局部仿射矩阵的关系表达式，根据所述关系表达式和所述归一化齐次图像坐标表示，确定所述局部仿射矩阵与所述对极约束的仿射矩阵约束，包括：

根据所述基本矩阵，确定所述基本矩阵和所述局部仿射矩阵的关系表达式为：

式中，()_(1:2)表示方程式系统的第一和第二方程式；

为一个3×3的矩阵，A表示局部仿射矩阵；

根据所述关系表达式和所述归一化齐次图像坐标表示，确定所述局部仿射矩阵与所述对极约束的仿射矩阵约束为：

式中，符号含义同公式(1)和(5)。

5.根据权利要求1～4任一项所述的方法，其特征在于，所述预设运动状态为一般运动状态；

所述获取预设运动状态下多像机系统的外参数，包括：

获取预设运动状态下多像机系统的外参数，得到旋转量R和平移量t分别为：

t＝[t_x t_y t_z]^T (8)

式中，q_x,q_y,q_z,t_x,t_y,t_z表示未知参量；

所述确定多像机系统的系统方程，包括：

确定多像机系统的系统方程为：

式中，M_ij表示在一般状态下系数矩阵M(q_x,q_y,q_z的元素，i＝1～3,j＝1～4；q_x,q_y,q_z,t_x,t_y,t_z表示未知参量；

所述根据至少一个仿射匹配点表示对应的仿射矩阵约束和所述对极约束，确定所述系统方程中待求解未知参量的值，包括：

根据两个仿射匹配点表示对应的仿射矩阵约束和所述对极约束，采用

工具确定所述系统方程中待求解6个未知参量的值。

6.根据权利要求1～4任一项所述的方法，其特征在于，所述预设运动状态为已知垂直方向运动状态；

所述获取预设运动状态下多像机系统的外参数，得到旋转量和平移量，包括：

获取预设运动状态下多像机系统的外参数，得到旋转量R_imu和3D平移量

为：

式中，θ_p和θ_r分别表示耦合IMU提供的侧倾角和俯仰角；R_imu、R’_imu分别表示在已知垂直方向运动状态下连续帧的旋转量；t表示在一般运动状态下的平移量；

所述确定多像机系统的系统方程，包括：

确定多像机系统的系统方程为：

式中，

表示在已知垂直方向运动状态下系数矩阵

的元素，i＝1～4,j＝1～4；

表示未知参量；

所述根据至少一个仿射匹配点表示对应的仿射矩阵约束和所述对极约束，确定所述系统方程中待求解未知参量的值，包括：

判断

的行列式是否为零；

若是，

的扩展式为：

式中，quot(a,b)表示计算a除以b的商；

由仿射匹配点对之间的Plücker线和局部仿射矩阵系数构成；q_y表示未知参量；

求解所述扩展式，得到一个最多有6个实根的闭式解，根据所述闭式解确定所述系统方程中待求解未知参量的值。

7.根据权利要求1～4任一项所述的方法，其特征在于，所述预设运动状态为已知垂直方向运动状态；

所述获取预设运动状态下多像机系统的外参数，得到旋转量和平移量，包括：

获取预设运动状态下多像机系统的外参数，得到旋转量R_imu和3D平移量

为：

式中，θ_p和θ_r分别表示耦合IMU提供的侧倾角和俯仰角；R_imu、R’_imu分别表示在已知垂直方向运动状态下连续帧的旋转量；t表示在一般运动状态下的平移量；

所述确定多像机系统的系统方程，包括：

确定多像机系统的系统方程为：

式中，

表示在已知垂直方向运动状态下系数矩阵

的元素，i＝1～4,j＝1～4；

表示未知参量；

所述根据至少一个仿射匹配点表示对应的仿射矩阵约束和所述对极约束，确定所述系统方程中待求解未知参量的值，包括：

判断

的行列式是否为零；

若是，

的扩展式为：

式中，quot(a,b)表示计算a除以b的商；

由仿射匹配点对之间的Plücker线和局部仿射矩阵系数构成；q_y表示未知参量；

求解所述扩展式，得到一个最多有6个实根的闭式解，根据所述闭式解确定所述系统方程中待求解未知参量的值。

8.一种基于仿射匹配点对的多像机系统相对位姿估计装置，其特征在于，所述装置包括：

外参数处理模块，用于获取预设运动状态下多像机系统的外参数；所述外参数包括旋转量和平移量；

确定所述多像机系统中连续帧之间的仿射匹配点表示；所述仿射匹配点表示包括：仿射匹配点对和局部仿射矩阵；

约束获取模块，用于根据所述仿射匹配点对和所述外参数，确定多像机系统对应的多像机参考坐标系中的归一化齐次图像坐标表示，根据所述归一化齐次图像坐标表示、旋转量和平移量，确定所述多像机系统中的对极约束；根据所述外参数，确定所述像机机系统中连续帧之间的转换矩阵，根据所述转换矩阵，确定连续帧之间的基本矩阵；根据所述基本矩阵，确定所述基本矩阵和所述局部仿射矩阵的关系表达式，根据所述关系表达式和所述归一化齐次图像坐标表示，确定所述局部仿射矩阵与所述对极约束的仿射矩阵约束；

系统方程获取模块，用于确定多像机系统的系统方程；所述系统方程中包括：待求解未知参量；

系统方程求解模块，用于根据至少一个仿射匹配点表示对应的仿射矩阵约束和所述对极约束，确定所述系统方程中待求解未知参量的值；

位姿估计模块，用于根据待求解未知参量的值确定预设运动状态下的相对位姿。

9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1～7中任一项所述方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1～7中任一项所述方法的步骤。

Images