CN113361400A - 一种头部姿态估计方法、装置及存储介质 - Google Patents

Abstract

本发明公开了一种头部姿态估计方法、装置及存储介质，包括：对构成图像金字塔的图像进行特征点的提取、优化和匹配，以确定当前帧的第二特征点的位置，估计头部姿态。本发明的有益效果是：通过对接收的图像进行特征提取，并不断划分图像以优化特征点，解决了特征点扎堆的现象，以较低的成本实现对头部姿态的跟踪，获得头部姿态相对关键帧偏转的角度。

Description

一种头部姿态估计方法、装置及存储介质

技术领域

本申请属于跟踪和目标姿态估计技术领域，特别是涉及一种头部姿态估计方法、装置及存储介质。

背景技术

随着计算机视觉领域的快速发展，头部姿态估计仍然是计算机视觉领域的研究热点之一。头部姿态估计简要的说就是通过输入图像或视频来判断出人的头部在三维空间中的三个姿态角度，即滚转角(roll)，俯仰角(pitch)，偏航角(yaw)，是某一时刻人的头部朝向的特写。头部姿态在人脸识别、人机交互、疲劳驾驶、注意力检测等方面都有着非常广泛的应用。通过不同头部姿态不仅能够推断出检测者的意愿，还可以得到其头部朝向进而判断检测者注意力是否集中，头部姿态估计在实际应用中有着非常广泛的发展前景。

目前，头部姿态估计按照实现方法的不同主要可分为外观模板方法、检测器阵列方法、非线性回归方法、几何计算方法、柔性模板方法，跟踪法和其它方法等。但目前头部姿态的估计方法存在鲁棒性差、精度低、实时性差等问题。

发明内容

本发明为了解决现有技术的不足，提供了一种头部姿态估计方法、装置及存储介质。

本发明提出一种头部姿态估计方法，包括：接收具有头部的图像，所述图像为双目相机拍摄的图像；以每帧所述图像所构建的图像金字塔作为节点提取第一特征点；将所述节点分割成4个子节点，若所述子节点中的第一特征点数量为0，则删去所述子节点；若所述子节点的第一特征点的数量为1，则保留该子节点；若所述子节点中的第一特征点数量大于1，则继续分割所述子节点成4个新的子节点，直至保留的子节点的数目大于第一预设值或保留的子节点中的第一特征点的数量均为1；保留每个子节点中Harris响应值最高的第一特征点，设置为第二特征点；匹配同一帧下的第二特征点，与该第二特征点绑定的世界坐标系的三维点，设置为地图点；根据所述地图点获取所述第二特征点在所述当前帧的位置；根据所述世界坐标系的关键帧与当前帧估计头部姿态。

在该技术方案中，通过对接收的图像进行特征提取，并不断划分图像以优化特征点，解决了特征点扎堆的现象，以较低的成本实现对头部姿态的跟踪，获得头部姿态相对关键帧偏转的角度。

在上述技术方案中，接收第一帧的头部偏转角度信息，以所述第一帧为关键帧，建立所述世界坐标系。

在该技术方案中，以图像的第一帧作为关键帧建立世界坐标系，有利于对其他帧的角度进行计算，有利于实现头部姿态偏转角度的估计。

在上述技术方案中，在执行根据所述地图点接收所述第二特征点在所述当前帧的位置后，储存所述关键帧和所述地图点。

在该技术方案中，保存关键帧有利于提高头部姿态角度的估计效率。

在上述技术方案中，若接收的所述图像与上一次接收的所述图像的场景相同，则调用储存的关键帧和地图点，对头部进行重定位和跟踪。

在该技术方案中，通过对储存的关键帧的调用，以实现重定位，可以提高头部的跟踪效率。

在上述技术方案中，所述匹配同一帧下的第二特征点，包括：在所述第二特征点取邻域S，得：

其中，N为点对个数，τ(S；p，q)指的是在提取到的所述第二特征点取一个邻域区块S，在这个邻域区块S中随机取点对p和q，f_N(S)为特征点描述子，i为正整数。

在该技术方案中，通过上述方程获取每个第二特征点的信息，有利于实现第二特征点的匹配，实现头部姿态偏转角度的估计，提高估计的准确度。

在上述技术方案中，所述根据所述地图点获取所述第二特征点在所述当前帧的位置，包括：分别接收前两帧的位姿；根据所述前两帧的位姿差计算当前帧的位姿；将前一帧中在所述世界坐标系的所述地图点投影至所述当前帧中，在以投影点为圆心、半径为r₁的范围内，搜索与所述前一帧的所述第二特征点相匹配的点，作为所述当前帧的第二特征点，其中，r₁与该地图点的第二特征点在所述图像金字塔的层数呈正相关。

在该技术方案中，通过前两帧的位姿差确定当前帧的第二特征点的位置，实现了对头部姿态的跟踪，有利于实现头部姿态偏转角度的估计，提高估计的准确度。

在上述技术方案中，所述分别接收前两帧的位姿，还包括：优化接收的位姿，其优化方程为：

其中，位姿为{R，t}，其李群表示为T，T^*是优化后的位姿，n为所有匹配的结果，ρ为鲁棒核函数，u_i为投影点，π_s为投影方程，P_i为地图点，P_i＝[X_i，Y_i，Z_i]^T，∑为与第二特征点尺度相关的协方差矩阵，π_s的定义如下：

其中，f_x，f_y为相机焦距，c_x，c_y为相机主点，b为相机基线距离。

在该技术方案中，优化前两帧的姿态有利于实现头部姿态偏转角度的估计，提高估计的准确度。

在上述技术方案中，将所述当前帧与所述关键帧组成局部地图，将所述局部地图的地图点投影到当前帧，以投影点为圆心、半径为r₂的范围内，搜索与所述前一帧的所述第二特征点相匹配的点，其中，r₂与所述局部地图的地图点在所述图像金字塔的层数呈正相关。

在该技术方案中，通过对局部地图的第二特征点的匹配，以提高当前帧第二特征点位置的准确度，有利于实现头部姿态偏转角度的估计，提高估计的准确度。

本发明还提供一种装置，包括存储器、处理器和程序，所述述程序被存储在所述存储器中，所述处理器执行所述程序以实现如前所述的方法。

本发明还提供一种存储介质，所述存储介质存储有程序，当所述程序被处理器执行，实现上述的头部姿态估计方法。

本发明的有益效果是：通过对接收的图像进行特征提取，并不断划分图像和处理特征点，解决了特征点扎堆的现象，以较低的成本实现对头部姿态的跟踪，并获得头部姿态相对关键帧偏转的角度。

附图说明

图1示出了根据本发明的一个实施例的头部姿态跟踪方法；

图2示出了根据本发明的一个实施例的双目相机成像的模型图；

图3示出了根据本发明的一个实施例的双目相机的几何模型图；

图4示出了根据本发明的一个实施例的跟踪算法的流程图；

图5示出了根据本发明的一个实施例的匀速模型匹配策略图；

图6示出了根据本发明的一个实施例的重投影误差图；

图7示出了根据本发明的一个实施例的相机坐标系与头部坐标系图；

图8示出了根据本发明的一个实施例的第二特征点的匹配流程图。

具体实施方式

为了可以更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节，以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

相关技术中，头部姿态是指人的头部相对于正脸图像在三维空间中的偏转角度，头部姿态估计是从数字图像或视频图像中推断出人的头部偏转角度的过程，具体发生偏转的角可以分为滚转角(roll)，偏航角(yaw)，俯仰角(pitch)。理想情况下，头部姿态在三个坐标轴方向的范围为-90°～90°，但实际上，通常对于成年人的头部偏转是有一定范围的，头部围绕X轴(Pitch)方向的头部姿态偏转范围为-60.4°～+69.6°，围绕Y轴(Yaw)方向的头部姿态偏转范围为-40.9°～36.3°，围绕Z轴(Roll)方向的头部姿态偏转范围为-79.8°～+75.37°。

下面参照附图描述本发明的一些实施例。以下对本发明作进一步详细说明。

如图1所示，本发明提供一种头部姿态估计方法，包括：

接收具有头部的图像，所述图像为双目相机拍摄的图像；

以每帧所述图像所构建的图像金字塔作为节点提取第一特征点；

将所述节点分割成4个子节点，若所述子节点中的第一特征点数量为0，则删去所述子节点；

若所述子节点的第一特征点的数量为1，则保留该子节点；

若所述子节点中的第一特征点数量大于1，则继续分割所述子节点成4个新的子节点，直至保留的子节点的数目大于第一预设值或保留的子节点中的第一特征点的数量均为1；

保留每个子节点中Harris响应值最高的第一特征点，设置为第二特征点；

匹配同一帧下的第二特征点，与该第二特征点绑定的世界坐标系的三维点，设置为地图点；

根据所述地图点获取所述第二特征点在所述当前帧的位置；

根据所述世界坐标系的关键帧与所述当前帧估计头部姿态。

通过对接收的图像进行特征提取，并不断划分图像以优化特征点，解决了特征点扎堆的现象，以较低的成本实现对头部姿态的跟踪，获得头部姿态相对关键帧偏转的角度。

可选的，图像金字塔的层数为8。

第一预设值应根据实际情况确定，设定提取到某个数量的特征点时效果能够保持相对比较稳定。

由于算法需要进行初始化，在世界坐标系中，可以设置第一帧的角度信息为正脸图像，以正脸图像为第一帧。正脸图像是指脸部正对左目相机的图像。可以理解的是，每一帧图像应包括左目相机拍摄的图像和右目相机拍摄的图像，以实现后续的第二特征点的匹配及其对应的地图点的绑定。

具体的，在其中一个实施例中，第二特征点的匹配过程如图8所示。

构建图像金字塔，图像金字塔的构建形成一系列不同分辨率的图像，有利于形成对人脸大小的认知。

限定提取区域，设定特征点提取区域为图像中央，可以减小特征点检测范围。

划分图像区块，对设定的提取区域分割成大小一致的图像网格，在每个图像网格内进行第一特征点提取。第一特征点的提取方式为：取子节点中的任意一个像素点a，设像素值为I_a，以点a为圆心选取半径为3的圆，将圆上的16个像素点分别与像素点a进行灰度值比较，如果有连续12个像素点的亮度大于I_a+T或小于I_a-T，则认为是第一特征点。为了提高效率，对于每个像素点，添加预测试操作，检测邻域圆上的第1，5，9，13个像素的亮度，当这4个像素中有3个同时大于I_a+T或小于I_a-T才可能是第一特征点。T的取值应根据实际情况选定，以在合适的提取范围提取第一特征点。

具体的，这种提取方式往往会带来众多的第一特征点，第一特征点之间往往会由于灰度值比较的范围过小而出现扎堆的现象，所以将每帧图像所构成的图像金字塔作为节点分成4个子节点，依据子节点内第一特征点的数量进行操作：删去没有第一特征点的子节点，可以避免对没有第一特征点的子节点进行重复的比较，提高效率；保留只有1个第一特征点的子节点，有利于利用第一特征点实现对当前帧头部姿态的估计；对具有1个以上第一特征点的子节点再次进行划分，直至子节点的数量达到第一阈值，或在每个子节点中第一特征点的数量均为1个。对扎堆的第一特征点进行操作，这种方式可以消除由于提取方式带来扎堆现象。另一方面，每个子节点中Harris响应值最高的第一特征点作为第二特征点进行保留，有利于后续头部姿态估计的运算。

Harris响应值是指运用哈里斯边角侦测算法得到的响应值，其大小可以评价像素点是否为第一特征点。哈里斯边角侦测(Harris Corner Detector)是被广泛运用在电脑视觉的算法，主要是用于从影像中找出代表边角的特征点

若图像发生旋转时，第二特征点的描述子信息会发生改变，可以通过灰度质心法计算特征点的主方向优化处理第二特征点，实现第二特征点的旋转不变性。

对于优化处理后的第二特征点，可以通过获取第二特征点的描述子信息提高第二特征点的匹配速率。BRIEF描述子(全称为Binary Robust Independent ElementaryFeatures，是一种二进制编码的描述子)。以方程

对每个点计算BRIEF描述子，其中，N为点对个数，N的取值可以为128，256，512中的一个，τ(S；p，q)指的是在提取到的所述第二特征点取一个邻域区块S，在这个邻域区块S中随机取点对p和q，f_N(S)为特征点描述子，i为正整数。获取描述子信息的第二特征点有利于完成第二特征点的匹配。

如图2所示，O₁和O₂为左右相机光圈中心，P为三维空间中的一点，由于双目相机获取的照片为同一对象在两个视角的图像，对于计算BRIEF描述子后的第二特征点，其匹配方法为：根据对极约束原理，与左图I₁上第二特征点P₁匹配的第二特征点P₂一定在右图I₂中的极线上，通过极线纠正后，P₂点只需要在I₂的x轴方向上搜索。

对于左目相机中的第二特征点，在右目相机中依据图像尺度设置不同大小的搜索带，得到候选特征点，通过与候选特征点进行BRIEF描述子匹配，再通过SAD匹配(Sum ofabsolute differences，一种图像匹配算法。基本思想：差的绝对值之和。此算法常用于图像块匹配，将每个像素对应数值之差的绝对值求和，据此评估两个图像块的相似度。)进行相似度比较，将不满足要求的误匹配点剔除，再通过抛物线拟合实现亚像素精度匹配，从而得到精确的匹配结果。

对于完成匹配的点，可以通过双目匹配关系绑定地图点，如图3所示，对于三维空间中的一点P，在左目相机和右目相机的成像点为P_L和P_R，其中，u_L和u_R为点P在不同成像平面上的u轴的像素坐标，f为焦距，O_L和O_R分别为左右相机光圈中心。根据相似三角形原理可以得到：

整理，得

其中，d＝u_L-u_R。通过上述绑定方法可以实现对世界坐标系中的三维点与完成匹配的第二特征点的绑定，完成绑定的三维点，成为地图点，可以用于确定与地图点绑定的第二特征点在当前帧内的位置。

在其中一个实施例中，确定与地图点绑定的第二特征点在当前帧内的位置的流程如图4所示。以下对每个流程进行介绍。

地图初始化阶段，完成第二特征点的匹配后，设三维点的深度值为z，双目相机基线为b，对深度值0＜z＜10*b的三维点与第二特征点绑定，成为带有第二特征点的观测信息、描述子信息和观测方向信息的地图点。将初始化成功的第一帧作为关键帧和后续跟踪的临时参考关键帧，建立初始世界坐标系。

跟踪匹配阶段，如图5所示，为获取所述第二特征点在当前帧的位置，分别接收前两帧的位姿；根据所述前两帧的位姿差计算当前帧的位姿；将前一帧中在所述世界坐标系的所述地图点投影至所述当前帧中，在以投影点为圆心、半径为r₁的范围内，搜索与所述前一帧的所述第二特征点相匹配的点，作为所述当前帧的第二特征点，其中，r₁与该地图点的第二特征点在所述图像金字塔的层数呈正相关。具体的，可以先基于DBoW2算法(全称为Bags of binary words 2，是基于Bags of binary words改进的方法。)对同一个节点上的特征点进行匹配，并将对应的地图点与匹配上的特征点进行绑定，得到3D-2D的匹配关系，根据前两帧之间的位姿差，基于匀速运动假设，设定当前帧的初始位姿。对于上一帧的地图点，投影至当前帧中，在半径r₁范围内搜索匹配点，得到最佳匹配结果。

如图6所示，对于通过投影的方式得到的匹配关系，可以采用BA(全称为BundleAdjustment，是一个优化模型，其目的是最小化重投影误差)构建最小化重投影误差约束，LM算法(全称为Levenberg-Marquarelt，是一种迭代求函数极值的算法，LM算法综合了牛顿法求极值与梯度法求极值这两种算法的特点。)迭代优化位姿，其中，重投影误差如图所示，在初始值中，通过匹配的第二特征点p₁所得到的地图点P，其投影点

与实际的p₂之间有一定的距离e，于是考虑多个点进行相机位姿的优化，使所有点的整体误差达到最小。设地图点为Pi＝[X_i，Y_i，Z_i]^T，投影点为u_i，投影方程为π_s，相机的位姿为{R，t}，其李群表示为T，n为所有匹配的结果，则可以得到优化方程：

其中，i在地图点的数量范围内取值，T^*是优化后的相机位姿，ρ为鲁棒核函数，用于抑制误差较大的投影点，∑为与特征点尺度相关的协方差矩阵。π_s的定义如下：

其中，f_x为相机在X方向的焦距，f_y为同一个相机在Y方向的焦距；c_x为相机在X方向的主点，c_y为同一个相机的Y方向的主点；b为两个相机的基线距离。

局部地图跟踪阶段，为了提高位姿精度，第一次BA优化后，将局部地图点投影到当前帧，获得更多3D-2D匹配关系，再次进行BA优化位姿。其中，局部地图由与当前帧存在共视关系的关键帧组成。将局部地图点投影到当前帧需要满足以下条件：

a)计算地图点的投影位置，超出图像边界则放弃；

b)计算当前观测视线与地图点平均观测方向的夹角余弦值，小于cos60°则放弃；

c)计算地图点到相机光心的距离d，如果

则放弃，其中d_min为观测到该点的距离下限，d_max为观测到该点的距离上限。

如果存在候选匹配特征点，则将三维点与未匹配特征点匹配得到3D-2D匹配关系。

关键帧决策阶段，由于跟踪的过程是连续的过程，当关键帧与当前帧的间隔过大时，需要考虑更换关键帧，其条件如下：

a)距离上一次重定位超过30帧

b)距离上一次生成关键帧超过30帧。

c)当前帧跟踪上的地图点＜0.4×参考关键帧跟踪上的地图点或当前帧跟踪上的地图点＜0.4×当前帧的总近点数。

d)当前帧跟踪上的点不小于15个。

其中，a)和b)中的帧数设置应该相同，且此处帧数仅为示例，可以根据实际情况选取其他帧数。由于以重定位成功的那一帧为关键帧，该帧数的设置生成关键帧时间间隔相关，过小的时间间隔会影响效率。参考关键帧是在关键帧中挑选出来比较具有代表性的，与当前帧关系比较密切的帧，这样当匀速模型跟踪丢失时，会先考虑跟踪参考关键帧，只有当都失败时才进行重定位。近点数就是在当前帧下深度值小于一定阈值的点被认为是近点。

同时，如果关键帧的数量超过设定阈值，进行冗余关键帧的判断，如果某个关键帧90％以上的地图点能被连续超过3帧关键帧同时观测到，则为冗余关键帧，将该关键帧剔除，从而保证不会随着时间的积累影响运行效率。

对于跟踪丢失，在局部键帧中搜索与当前帧相似度最高的关键帧进行重定位，采用DBoW2加速匹配，EPNP算法(全称为Efficient Perspective-n-Point，是一种求解摄像头位姿的非迭代的PnP算法)计算初始位姿并通过BA最小化重投影误差优化，再将关键帧中未匹配的地图点投影到当前帧再次进行BA位姿优化。

正常跟踪结束，对地图进行保存，包括关键帧以及关键帧中各第二特征点的匹配信息和绑定信息，这些存储的信息如果下次在同一场景运行时可以选择加载进来，从而跳过初始化步骤直接实现重定位并继续跟踪位姿。只要是在双目相机没有发生移动的情况下，在对同一头部进行追踪和定位后，再次对该头部进行跟踪，则视为两次跟踪属于同一场景，当在同一场景下运行算法时，会在这个人头部重新提取特征点，然后与存储的关键帧进行匹配，如果有足够多的点匹配上了，则重定位成功，从而可以进行后续的跟踪和头部姿态的估计。

根据所述世界坐标系的关键帧与所述当前帧估计头部姿态，可以实现对头部姿态角度的估计。其中，引用到旋转矩阵和欧拉角以对头部姿态的估计，旋转矩阵描述的是：一个刚体在三维空间中的旋转，可以看成是三个自由度的运动，也就是分别绕三个轴旋转，这三个转动的角为欧拉角，对于始终以原始坐标系为基准进行的转动，可以称为静态欧拉角。

定义坐标系，其中，绕Z轴转动对应Roll角，记为α，绕Y轴转动对应Yaw角，记为β，绕X轴转动对应Pitch角，记为γ。对于三次分别绕固定轴的旋转，分别有如下定义：

旋转矩阵定义如下：

由旋转矩阵与静态欧拉角的转换关系可以得出R(α，β，γ)＝R_z(α)R_y(β)R_x(γ)，则有：

其中c表示cos，s表示sin，从而得到α，β，γ角的计算公式如下：

α＝atan2(R₂₁，R₁₁)

γ＝atan2(R₃₂，R₃₃)

由图7可知，以人脸正对左目相机时的图像建立初始世界坐标系，对于头部坐标系，α应取为-α，β应取为-β，而γ应保持不变，忽略相机与头部模型之间的平移，从而可以得到头部姿态，即滚转角、偏航角、俯仰角。

为了定量地评价头部姿态估计的准确性，采用评价指标平均绝对误差(MAE)以及标准差(Standard Deviation)，分别在roll[-15°，15°]，pitch[-15°，15°]，yaw[-15°，15°]上进行测试，并对多次试实验结果取平均值。其中，平均绝对误差是绝对误差的平均值，能更好地反映出预测值偏离真实值的误差实际情况，平均绝对误差计算公式如下：

式中，α′，β′，γ′为测量值，式中，α，β，γ为真实值。

采用标准差(SD)，记作σ，作为头部姿态估计算法的评价标准之一，标准差是用来衡量一组数据偏离平均数的离散程度，由于本发明创造采用的是连续的视频图像序列，因此标准差能够表明连续跟踪时的数据稳定性，标准差的计算公式如下：

式中，α′，β′，γ′为测量值，u_α，u_β，u_γ为平均值。

在其中一个实施例中，使用双目ZED相机进行数据采集，ZED相机的深度范围为0.3-25m，采集相机到头部模型的距离约45cm。首先获取该双目相机的内外参数矩阵，计算内外参数的，然后使用ZED相机录制头部模型视频，帧率为15fps，图像大小为1280×720，由于该仪器只能调节15°范围内的转动，因此本次实验采集的数据均在[-15°，15°]之间。俯仰角、偏航角、滚转角分别采集视频，再将视频转换640×360大小的图片作为算法输入。由于算法需要进行初始化，可以设置第一帧的角度信息为正脸图像，以正脸图像为第一帧。

本发明还提供一种装置，包括存储器、处理器和程序，所述述程序被存储在所述存储器中，所述处理器执行所述程序以实现如前所述的方法。

本发明还提供一种存储介质，所述存储介质存储有程序，当所述程序被处理器执行，实现上述的头部姿态估计方法。

Claims (10)

1.一种头部姿态估计方法，其特征在于，包括：

接收具有头部的图像，所述图像为双目相机拍摄的图像；

以每帧所述图像所构建的图像金字塔作为节点提取第一特征点；

将所述节点分割成4个子节点，若所述子节点中的第一特征点数量为0，则删去所述子节点；

若所述子节点的第一特征点的数量为1，则保留该子节点；

若所述子节点中的第一特征点数量大于1，则继续分割所述子节点成4个新的子节点，直至保留的子节点的数目大于第一预设值或保留的子节点中的第一特征点的数量均为1，保留每个子节点中Harris响应值最高的第一特征点，设置为第二特征点；

匹配同一帧下的第二特征点，与该第二特征点绑定的世界坐标系的三维点，设置为地图点；

根据所述地图点获取所述第二特征点在所述当前帧的位置；

根据所述世界坐标系的关键帧与所述当前帧估计头部姿态。

2.根据权利要求1所述的头部姿态估计方法，其特征在于，接收第一帧的头部偏转角度信息，以所述第一帧为关键帧，建立所述世界坐标系。

3.根据权利要求1所述的头部姿态估计方法，其特征在于，在执行根据所述地图点接收所述第二特征点在所述当前帧的位置后，储存所述关键帧和所述地图点。

4.根据权利要求3所述的头部姿态估计方法，其特征在于，若接收的所述图像与上一次接收的所述图像的场景相同，则调用储存的关键帧和地图点，对头部进行重定位和跟踪。

5.根据权利要求1所述的头部姿态估计方法，其特征在于，所述匹配同一帧下的第二特征点，包括：在所述第二特征点取邻域S，得：

其中，N为点对个数，τ(S；p，q)指的是在提取到的所述第二特征点取一个邻域区块S，在这个邻域区块S中随机取点对p和q，f_N(S)为特征点描述子，i为正整数。

6.根据权利要求5所述的头部姿态估计方法，其特征在于，所述根据所述地图点获取所述第二特征点在所述当前帧的位置，包括：

分别接收前两帧的位姿；

根据所述前两帧的位姿差计算当前帧的位姿；

将前一帧中在所述世界坐标系的所述地图点投影至所述当前帧中，在以投影点为圆心、半径为r₁的范围内，搜索与所述前一帧的所述第二特征点相匹配的点，作为所述当前帧的第二特征点，其中，r₁与该地图点的第二特征点在所述图像金字塔的层数呈正相关。

7.根据权利要求6所述的头部姿态估计方法，其特征在于，所述分别接收前两帧的位姿，还包括：优化接收的位姿，其优化方程为：

其中，位姿为{R，t}，其李群表示为T，T^*是优化后的位姿，n为所有匹配的结果，ρ为鲁棒核函数，u_i为投影点，π_s为投影方程，P_i为地图点，P_i＝[X_i，Y_i，Z_i]^T，∑为与第二特征点尺度相关的协方差矩阵，π_s的定义如下：

其中，f_x，f_y为相机焦距，c_x，c_y为相机主点，b为相机基线距离。

8.根据权利要求6所述的头部姿态估计方法，其特征在于，所述根据所述地图点接收所述第二特征点在所述当前帧的位置，包括：

将所述当前帧与所述关键帧组成局部地图，将所述局部地图的地图点投影到当前帧，以投影点为圆心、半径为r₂的范围内，搜索与所述前一帧的所述第二特征点相匹配的点，其中，r₂与所述局部地图的地图点在所述图像金字塔的层数呈正相关。

9.一种装置，其特征在于，包括存储器、处理器和程序，所述述程序被存储在所述存储器中，所述处理器执行所述程序以实现如权利要求1-8中任一项所述的方法。

10.一种存储介质，其特征在于，所述存储介质存储有程序，当所述程序被处理器执行，实现上述的头部姿态估计方法。

Images