CN111476842B - 一种像机相对位姿估计方法及*** - Google Patents

Abstract

本发明公开一种像机相对位姿估计方法及***，该方法包括：利用仿射不变特征描述子在两视图之间建立多个仿射匹配点对；根据运动约束条件构建约束方程，利用单个仿射匹配点求解约束方程闭合形式解获得两视图之间的相对位姿；通过获得的相对位姿结合RANSAC框架剔除仿射匹配点对中的误匹配点对，确定仿射匹配点对内点；利用两视图之间仿射匹配点对的内点优化相对位姿，提高像机相对位姿估计的精度。用于解决现有技术中必须要多个图像匹配点对导致计算效率低、耗费大量计算资源等问题，采用单个仿射匹配点对估计位姿，提高计算效率，降低资源配置。

Description

一种像机相对位姿估计方法及***

技术领域

本发明涉及位姿解算技术领域，具体是一种通过两幅视图对像机相对位 姿进行估算的方法及***。

背景技术

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

典型的SLAM和SfM***都包含以下主要步骤：首先，通过特征匹配算法 来建立视图之间的图像匹配点对；然后，采用随机抽样一致(RANdom SAmple Consensus,RANSAC)等算法剔除图像匹配点对中的误匹配点对。最后，利用图 像匹配点对中的内点求解视图之间的相对位姿关系。其中误匹配点对剔除对 于相对位姿估计算法的精度和鲁棒性至关重要，同时误匹配点对剔除的效率 直接影响SLAM和SfM***的实时性能。由于在相同的野值比例条件下， RANSAC的随机采样次数随着最小配置解所需图像匹配点对数量的增加而成 指数型增长。而当前，主流的相对位姿估计算法通过SIFT，SURF等特征描述 子获得图像匹配点对。针对三维场景，利用5个图像匹配点对求解两视图之 间的本质矩阵；针对平面场景，利用4个图像匹配点对求解两视图之间的单应矩阵。进而通过分解本质矩阵或单应矩阵，求解视图之间的相对位姿关系。 因此，研究获得相对姿态估计的最小配置解显得非常重要。

发明内容

本发明提供一种像机相对位姿估计方法及***，用于克服现有技术中需 要多个图像匹配点对导致占用大量计算资源等缺陷，通过单个仿射匹配点对 的信息对像机的相对位姿进行估算，减少求解像机相对位姿估计所需要的匹 配点对数量，实现最小配置解，提高计算效率，大大减小计算资源配置。

为实现上述目的，本发明提供一种像机相对位姿估计方法，包括：

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

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

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

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

为实现上述目的，本发明还提供一种像机相对位姿估计***，包括存储 器和处理器，所述存储器存储有像机相对位姿估计程序，所述处理器在运行 所述像机相对位姿估计程序时执行权利上述方法的步骤。

本发明提供的像机相对位姿估计方法及***，首先通过利用ASIFT等仿 射不变特征描述子建立两视图之间的仿射匹配点对；根据运动约束条件构建 约束方程，利用单个仿射匹配点求解约束方程闭合形式解获得两视图之间的 相对位姿，每个仿射匹配点对求解获得一个相对位姿，结合RANSAC框架将 获得同一相对位姿解数量最多的仿射点匹配点对作为仿射匹配点对内点，将其余仿射匹配点对作为误匹配点对剔除，最后利用视图之间仿射匹配点对的 内点进一步优化相对位姿，以提高像机相对位姿估计的精度。

附图说明

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

图1为本发明实施例一提供的像机相对位姿估计方法的流程图；

图2为实施例一、二、三中两个视图之间的仿射匹配点对，局部仿射矩 阵A描述了图像匹配点对(p_i,p_j)之间邻域信息的关系图；

图3为实施例一、二、三中像机平面运动的俯视图；

平面运动可有两个未知数描述：偏航角θ和平移方向角φ；

图4为实施例四种已知垂直方向的像机运动示意图；

相对位姿中的未知量包括偏航角θ和平移向量[t_x,t_y,t_z]^T

图5a为单目视觉里程计ORB-SLAM2***对KITTI00列数据集估计的轨 迹与地面真实轨迹对比图；

图5b为采用实施例三的方法对KITTI00列数据集估计的轨迹与地面真实 轨迹对比图。

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

具体实施方式

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

需要说明，本发明实施例中所有方向性指示(诸如上、下、左、右、前、 后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位 置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应 地随之改变。

另外，在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的， 而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数 量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少 一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个， 三个等，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等 应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或 成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信 连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内 部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的 普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

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

实施例

如图1所示，本发明实施例一提供一种像机相对位姿估计方法，具体包 括以下步骤：

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

如今，仿射不变特征描述子(affine-covariant feature detectors)如ASIFT 和MODS受到了越来越多的关注。ASIFT等仿射不变特征描述子提供两视图 之间的仿射匹配点对(Affine Correspondence)，其由图像匹配点对和对应的2×2 仿射矩阵组成，见图2。仿射匹配点对不仅包含两视图之间的图像匹配点对， 而且包含描述图像匹配点对之间领域信息的局部仿射矩阵。本发明充分利用 仿射匹配点对信息，利用单个仿射匹配点对估计像机在平面运动和已知垂直 方向条件下的相对位姿估计问题。

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

闭合形式解又叫闭式解，获得相对位姿与其他参数之间确定的关系式；

用ASIFT等仿射不变特征描述子提供两视图之间的仿射匹配点对，其由 图像匹配点对和对应的2×2仿射矩阵组成，单个仿射匹配点对对几何模型估计产生三个约束，从而计算出相对位姿估计最小配置解；本专利对实例一提 出了两种求解方法：闭式求解方法和最小二乘求解方法；对实例二和实例三 各提出了一种求解方法：闭式求解方法。

步骤S3，并结合RANSAC框架剔除仿射匹配点对中的误匹配点对，确定仿 射匹配点对内点；

结合RANSAC框架剔除仿射匹配点对中的误匹配点对的具体过程为：根据 单个仿射匹配点对解算获得的相对位姿并结合RANSAC框架，选取获得仿射匹 配点对数量最多的相对位姿解，将满足该相对位姿对极几何约束的仿射匹配点对作为内点保留；将其他仿射匹配点对作为误匹配点对剔除；RANSAC框架 为公知技术；每个仿射匹配点对带入步骤2建立的约束方程，通过求解均可 获得相对位姿，如果仿射匹配点对是两视图的内点，通过对极几何约束能够 判断其他内点，并剔除不满足该对极几何约束的外点。；如果仿射匹配点对 是两视图的外点，由于噪声的随机性，则仿射匹配点对通过上述约束方程求 解获得两视图的相对位姿，并不满足对极几何约束，，由此选取获得仿射匹 配点对数量最多的相对位姿解，将满足该相对位姿对极几何约束的对作为内点保留；将其他仿射匹配点对作为误匹配点对剔除；

步骤S4，利用两视图之间其他仿射匹配点对的内点优化相对位姿。

找到两视图的内点后，具体可通过现有技术的算法对两视图的相对位姿 进行优化(公知技术和流程：利用内点和位姿关系的初值，可以进行非线性 优化)，这里不做详述。重复上述步骤S1～4，即可获得像机的运动轨迹。

本发明的技术方案极大减少了相对位姿估计所需的点对数量，整体性能 好且旋转精度明显高于其他方法，可以有效地用于视觉里程计中的异常匹配 点对剔除和初始运动估计，在自动驾驶汽车和地面机器人操作的场景中具有 广泛的应用前景。关于步骤2下面提供以下几个具体实施例：

实施例一

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

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

如图2、3所示，像机为平面运动，且已做内参标定，已知像机内参数 条件下，视图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分别代表两视图之间的相对旋转 和平移关系。

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

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

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

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

此外，广泛使用的仿射不变特征描述子，例如ASIFT直接提供两个视 图之间的仿射匹配点对，通过充分利用仿射匹配点对信息，可进一步 减少相对位姿估计所需的匹配点对数量。

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

首先，我们介绍仿射匹配点对：(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)

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

采用闭式解方法对上述方程求解：

对于仿射点对，方程组(5)、(9)和(10)可以表示为 Cx＝0,x＝[sin(θ-φ),cos(θ-φ),sin(φ),cos(φ)]^T。为了便于描述下面的方法，我们 用记号表示：

忽略x项之间的隐式约束，即

和/>

x应属于零空间C, 因此，矩阵C^TC最小特征值对应的特征向量即为***x的解。。通过SVD 获得x，则角度θ和φ分别为：

实施例二

如图1～3所示，在实施例一的基础上，应用场景与实施例一相同，即像 机为平面运动，且内参标定，步骤S23a采用最小二乘解方法对上述方程求解， 获得两视图之间平面运动航偏角和平移方向角。过程如下：

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

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

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

通过令

和/>

的偏导数为零,我们得到一个含有未知数/>

和/>

的方程组。此方程组含有6个未知数{x₁,x₂,x₃,x₄,λ₁,λ₂},且秩为2。可 通过格罗布纳基(

basis)方法求解该方程组，格罗布纳基方法表明最 多有8个解。在RANSAC框架下，获得仿射匹配内点数量最多的解为最终的解。

实施例三

与上述实施例一、二的应用场景不同的是，像机同样做平面运动 但内参未标定，在本小节中，假设有一个像机，除了焦距未知外，它 的其他内参数是已知的。所述步骤S23a采用如下闭式解方法求解获得两视 图之间平面运动航偏角和平移方向角。

这种情况在实际中很常见。对于大多数像机来说，通常可以合理 地假设像元尺寸为正方形，并且主点在图像中心。假设像机内参数中 唯一未知的参数是焦距f，可将摄像机的内参数矩阵简化为 K＝diag(f,f,1)。由于内参数矩阵是未知的，因此我们无法得到归一化像 平面上图像点特征的坐标。而视图i和j中点的归一化齐次图像坐标分 别是p_i＝[u_i，v_i，1]^T和p_j＝[u_j,v_j,1]^T。在不失一般性的前提下，我们将主点 作为像平面的中心。将原始图像平面i和j中一个点的坐标分别标记为

和/>

且g＝f^-1，得到下列关系式

将式(16)代入(5)、(9)、(10)得到三个方程。为了减轻记法上的负担， 把式(11)代入这三个方程。通过将它们与两个三角约束结合起来，得到 了如下多项式方程组：

上面的方程组包含5个未知数{x₁,x₂,x₃,x₄,g}，秩为3。同样可通过格 罗布纳基(

basis)方法求解该方程组，格罗布纳基方法表明最多有6 个解。

实施例四

如图4所示，与上述实施例应用场景的不同之处在于，像机与惯性测量 单元固定，可通过惯性测量单元获得垂直方向运动参数，像机在三维空间内 运动：所述步骤S2采用如下闭式解方法求解获得两视图之间平面运动航偏角和平移方向角。

一个已知垂直方向条件的两视图相对运动估计最小解，它同样只使用单 个仿射匹配点对，参见图4。在这种情况下，假定惯性测量单元(IMU)与像 机固联安装。假设像机的俯仰角和滚转角可以直接从IMU中获得，从而可将 每个像机坐标系校正到垂直方向。像机的Y轴平行于重力方向，像机的X-Z 平面垂直于重力方向。像机坐标系转换到校正后像机坐标系的旋转矩阵R_imu表 示为:

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

用

和/>

分别表示由IMU提供的用于校正视图i和j的旋转矩 阵。则视图i和j中校正后的图像坐标可以表示为：

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

注意

表示校正后视图i和j之间的简化基本矩阵，其中/>

是 校正后视图i和j之间的平移方量，R_y是校正后视图i和j之间的旋转矩 阵。将公式(19)带入公式(7)：

通过将公式(20)的两边乘以旋转矩阵

产生一个方程

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

其中

表示校正后的图像匹配对点/>

和/>

之间的仿射 矩阵。

为了进一步推导，我们将

和/>

表示如下

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

此外，极线约束

可以写为：

对于仿射匹配点对(p_i,p_j,A)，方程组(24)～(26)可以表示为Mx＝0， 其中

是本质矩阵的未知元素向量。M的零空间是三维 的。可以通过三个零空间基向量的线性组合来确定恢复到尺度大小。

该多项式方程组x的解：

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

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

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

通过将(27)代入公式(28)和(29)，可以生成具有未知数β和γ的多 项式方程组。我们将方程组转化为关于γ的一元四次方程，进行求解β。 一旦获得了系数β和γ，简化的基本矩阵

就可以由(27)确定。并可 通过利用方程(23)分解得到R_y和/>

最后，视图i和j之间的相对姿态 可以通过以下公式获得

以实例三求解方法为例，通过把该求解方法融入到单目视觉里程计 ORB-SLAM2***中，以评估其在实际应用中的性能。首先通过ASIFT特征 匹配算法提取的仿射匹配点对，替换ORB特征。结合RANSAC框架，利用该求解方法估计两个连续帧之间的相对姿态，其用于替换原始***中的地图 初始化和匀速运动模型假设。图5a、图5b为对KITTI数据集进行实验的结果， 轨迹的颜色为绝对轨迹误差的编码，图5b右侧的灰度显示为轨迹误差和颜色之间的关系。图5a、图5b中灰色曲线是估计的轨迹，带有星号的黑色曲线是 地面真实轨迹。其中图5a中灰色轨迹线为单目视觉里程计ORB-SLAM2*** 估计的轨迹，可以看出与地面真实轨迹的误差较大，图5b中灰色轨迹线为将 本实施例三的方法融入到单目视觉里程计ORB-SLAM2***对相同数据进行 估计的轨迹，可以看出与地面真实轨迹误差较小，基本吻合，实验结果验证了利用所提出的像机位姿估计方法，能够有效提高单目视觉里程计的精度。 本发明可以达到以下的技术效果：

1)本发明针对像机在平面运动和已知垂直方向条件下的相对位姿估计问 题，充分利用视图之间的仿射匹配点对信息，极大减少了相对位姿估计所需 的点对数量。

2)本发明在像机平面运动假设下，提出了三种相对位姿估计最小配置解， 仅需要单个仿真匹配点对即可求解平面运动条件下的像机相对位姿。

3)本发明针对已知垂直方向的像对运动情况，提出了估计像机相对姿态 的最小配置解求解方法，同样仅需要单个仿真匹配点对；

4)本发明的方法可以有效地用于视觉里程计和三维重建等领域中的误匹 配点对的剔除和初始运动估计，在自动驾驶汽车和地面机器人操作的场景中 具有广泛的应用前景。

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

Claims (2)

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所述方法的步骤。

Images