一种像机相对位姿估计方法及系统与流程

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

背景技术：

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

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

技术实现要素：

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

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

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

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

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

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

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

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

附图说明

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

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

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

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

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

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

相对位姿中的未知量包括偏航角θ和平移向量[tx,ty,tz]^t

图5a为单目视觉里程计orb-slam2系统对kitti00列数据集估计的轨迹与地面真实轨迹对比图；

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

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

具体实施方式

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

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

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

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

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

实施例

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

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

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

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

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

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

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

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

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

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

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

实施例一

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

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

如图2、3所示，像机为平面运动，且已做内参标定，已知像机内参数条件下，视图i至j之间的对极约束如下所示

其中pi＝[ui,vi,1]^t，pj＝[uj,vj,1]^t分别是视图i和j中图像匹配点对的归一化图像坐标。e＝[t]×r是基本矩阵，r和t分别代表两视图之间的相对旋转和平移关系。

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

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

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

visin(θ-φ)+viujcos(θ-φ)+vjsin(φ)-uivjcos(φ)＝0.(5)

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

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

首先，我们介绍仿射匹配点对：(pi,pj,a)。局部仿射矩阵a描述了图像匹配点对(pi,pj)之间邻域信息的关系，定义如下：

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

其中ni＝e^tpj与nj＝epi分别表示视图i和j中的极线.定义是3×3矩阵：

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

a11vicos(θ-φ)+a21sin(φ)-(a21ui+vj)cos(φ)＝0,(9)

sin(θ-φ)+(a12vi+uj)cos(θ-φ)+a22sin(φ)-a22uicos(φ)＝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)的三角函数隐式约束可以重新表述为:

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

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

通过令和的偏导数为零,我们得到一个含有未知数和的方程组。此方程组含有6个未知数{x1,x2,x3,x4,λ1,λ2},且秩为2。可通过格罗布纳基(basis)方法求解该方程组，格罗布纳基方法表明最多有8个解。在ransac框架下，获得仿射匹配内点数量最多的解为最终的解。

实施例三

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

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

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

上面的方程组包含5个未知数{x1,x2,x3,x4,g}，秩为3。同样可通过格罗布纳基(basis)方法求解该方程组，格罗布纳基方法表明最多有6个解。

实施例四

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

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

式中θx和θz分别为俯仰角和滚转角。

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

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

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

通过将公式(20)的两边乘以旋转矩阵产生一个方程

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

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

为了进一步推导，我们将和表示如下

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

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

对于仿射匹配点对(pi,pj,a)，方程组(24)～(26)可以表示为mx＝0，其中是本质矩阵的未知元素向量。m的零空间是三维的。可以通过三个零空间基向量的线性组合来确定恢复到尺度大小。

该多项式方程组x的解：

x＝βm1+γm2+m3,(27)

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

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

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

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

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

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

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

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

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