CN118212258A - 位姿追踪方法以及***、设备及存储介质 - Google Patents

位姿追踪方法以及***、设备及存储介质 Download PDF

Info

Publication number
CN118212258A
CN118212258A CN202211623193.7A CN202211623193A CN118212258A CN 118212258 A CN118212258 A CN 118212258A CN 202211623193 A CN202211623193 A CN 202211623193A CN 118212258 A CN118212258 A CN 118212258A
Authority
CN
China
Prior art keywords
pose
light spot
spot
inertial measurement
measurement data
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211623193.7A
Other languages
English (en)
Inventor
牟文杰
毛文涛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Qualcomm Technologies Inc
Original Assignee
Qualcomm Technologies Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Qualcomm Technologies Inc filed Critical Qualcomm Technologies Inc
Priority to CN202211623193.7A priority Critical patent/CN118212258A/zh
Priority to PCT/CN2023/139055 priority patent/WO2024125625A1/en
Publication of CN118212258A publication Critical patent/CN118212258A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/77—Determining position or orientation of objects or cameras using statistical methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

一种位姿追踪方法以及***、设备及存储介质，方法包括：获取可移动设备的当前帧图像，可移动设备上设置有多个定位灯；基于当前帧图像，提取定位灯对应的光斑特征；获取惯性测量数据；判断位姿是否已初始化；如果是，则获取前一帧图像对应的位姿作为参考位姿，并基于光斑特征和参考位姿以及惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；否则，基于光斑特征，进行初始化搜索，用于获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；通过紧耦合的方式对光斑序号和惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。本发明实施例提升位姿追踪的鲁棒性，优化用户体验。

Description

位姿追踪方法以及***、设备及存储介质
技术领域
本发明实施例涉及数据处理技术领域，尤其涉及一种位姿追踪方法以及***、设备及存储介质。
背景技术
VR(Virtual Reality，虚拟现实)/AR(Augmented Reality，增强现实)/MR(MixedReality，混合现实)技术可以提供一种模拟的三维数字体验，允许我们身临其境地体验任何虚拟或真实的世界，而不受实际身处之地的约束。
6DoF(Degree of Freedom，自由度)手柄控制器(Handheld Controller)是VR/AR/MR中一种重要的交互方式，特别是像游戏这种需要高精度、高鲁棒性、低延迟的应用场景。随着VR/AR/MR技术的发展，头戴式6DoF一体机逐渐成为主流，而6DoF手柄控制器逐渐成为头戴式6DoF一体机的标配。
6DoF手柄控制器实现方式多种多样，根据使用的传感器，可以分为光学、电磁和超声波等方式。基于光学追踪的手柄，手柄灯环主动发出红外光，头显上的相机拍摄红外光图像，利用视觉算法计算得到手柄6DoF位姿。光学手柄的优势在于定位精度高、跟踪鲁棒性强、用户体验好，缺点是容易受到环境光照影响，容易受遮挡，并且追踪范围受限于头显上相机的FOV(Field of View，视场角)，无法360度全范围追踪，即存在追踪盲区。基于电磁和超声波传感器的手柄，优势是能够实现360度全范围追踪，但追踪过程中容易受到环境中电磁信号或者声波干扰，存在较多的局限性，导致手柄控制器在虚拟世界中，会有漂移、抖动、卡顿等现象，严重影响用户体验。
总体而言，基于光学追踪的精度最高。但是，目前位姿追踪的鲁棒性和用户体验有待提高。
发明内容
本发明实施例解决的问题是提供一种位姿追踪方法以及***、设备及存储介质，提升位姿追踪的鲁棒性，优化用户体验。
为解决上述问题，本发明实施例提供一种位姿追踪方法，包括：获取可移动设备的当前帧图像，所述可移动设备上设置有多个定位灯；基于所述当前帧图像，提取定位灯对应的光斑特征；获取惯性测量数据；判断位姿是否已初始化；如果是，则获取前一帧图像对应的位姿作为参考位姿，并基于所述光斑特征和所述参考位姿以及所述惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；否则，基于所述光斑特征，进行初始化搜索，用于获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；通过紧耦合的方式对光斑序号和惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
相应的，本发明实施例还提供一种位姿追踪***，包括：图像处理模块，用于获取可移动设备的当前帧图像，可移动设备上设置有多个定位灯；图像处理模块还用于基于所述当前帧图像，提取定位灯的光斑特征；惯性测量模块，用于获取惯性测量数据；初始化判断模块，用于判断位姿是否已初始化；跟踪匹配模块，用于在判断位姿已初始化时，获取前一帧图像对应的位姿作为参考位姿，并基于所述光斑特征和所述参考位姿以及所述惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；初始化搜索模块，用于在判断位姿未初始化时，基于光斑特征进行初始化搜索，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；视觉惯性融合模块，用于通过紧耦合的方式对所述光斑序号和所述惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
相应的，本发明实施例还提供一种设备，包括至少一个存储器和至少一个处理器，所述存储器存储有一条或多条计算机指令，其中，所述一条或多条计算机指令被所述处理器执行以实现本发明实施例所述的位姿追踪方法。
相应的，本发明实施例还提供一种存储介质，所述存储介质存储有一条或多条计算机指令，所述一条或多条计算机指令用于实现本发明实施例所述的位姿追踪方法。
与现有技术相比，本发明实施例的技术方案具有以下优点：
本发明实施例提供的位姿追踪方法，基于所述光斑特征和所述参考位姿以及所述惯性测量数据获得光斑序号，或基于光斑特征进行初始化搜索以获得光斑序号；通过紧耦合的方式对所述光斑序号和所述惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿，从而对视觉信息和惯性测量数据进行融合，惯性测量的帧率较高，有利于提高当前位姿的输出频率，而且，通过视觉信息和惯性测量数据的融合，还对当前位姿进行了平滑滤波，从而减小输出抖动；此外，惯性测量数据短时间内的精度较高，通过紧耦合的方式对光斑序号和惯性测量数据进行融合，还有利于将视觉信息和惯性测量数据相互弥补，不仅有利于缩小图像搜索范围、大幅度提升视觉跟踪的鲁棒性，而且还能够有效削除累积误差，进而提升位姿追踪方法的鲁棒性和优化用户体验。
可选方案中，基于光斑特征，进行初始化搜索的步骤包括：对光斑特征进行排序；获得最长的连续光斑序列；对连续光斑序列的起始点进行遍历搜索；根据当前连续光斑序列所对应的光斑序号，计算可移动设备的位姿信息；基于位姿信息，将当前连续光斑序列对应的定位灯投影至当前帧图像中，并计算重投影误差；获得重投影误差满足预设阈值要求的最佳连续光斑序列；基于最佳连续光斑序列，确定光斑对应的定位灯在可移动设备上的序号，作为光斑序号；本发明实施例利用定位灯之间的连续位置关系，进行初始化搜索，以确定光斑对应的定位灯的序号，有利于缩小搜索范围，进而大幅度缩短初始化搜索的时间。
可选方案中，基于所述光斑特征和初始位姿以及惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号的步骤包括：基于参考位姿和惯性测量数据，获得当前帧图像对应的预测位姿，预测位姿包括预测旋转信息；进行一次或多次迭代的跟踪匹配处理；其中，每次跟踪匹配处理包括：随机选取两个光斑特征，作为随机光斑特征；估计随机光斑特征对应的最佳光斑序号；基于最佳光斑序号和预测旋转信息，获得可移动设备的初始位置信息；基于预测旋转信息和初始位置信息，获得所有定位灯在图像中的投影；将投影与光斑特征进行匹配，判断匹配数量和重投影误差是否满足匹配要求；如果是，则判断匹配成功，并输出当前的光斑序号；否则，重复进行跟踪匹配处理；通过进行一次或多次迭代的跟踪匹配处理，采用两点RANSAC(Random Sample Consensus)的方法进行跟踪匹配处理，其中，基于随机光斑特征对应的最佳光斑序号以及预测旋转信息，有利于获得准确度较高的初始位置信息，相应有利于提高在将投影与光斑特征进行匹配的匹配度，而且，因为无需预测位姿中的预测位置信息进行跟踪匹配，还有利于容忍较大的预测位置信息误差，从而大幅度提高跟踪匹配的鲁棒性。
本发明实施例提供的位姿追踪***中，跟踪匹配模块基于所述光斑特征和所述参考位姿以及所述惯性测量数据获得光斑序号，或初始化搜索模块基于光斑特征进行初始化搜索以获得光斑序号；视觉惯性融合模块通过紧耦合的方式对所述光斑序号和所述惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿，从而对视觉信息和惯性测量数据进行融合，惯性测量的帧率较高，有利于提高当前位姿的输出频率，而且，通过视觉信息和惯性测量数据的融合，还对当前位姿进行了平滑滤波，从而减小输出抖动；此外，惯性测量数据短时间内的精度较高，通过紧耦合的方式对光斑序号和惯性测量数据进行融合，还有利于将视觉信息和惯性测量数据相互弥补，不仅有利于缩小图像搜索范围、大幅度提升视觉跟踪的鲁棒性，而且还能够有效削除累积误差，进而提升位姿追踪的鲁棒性和优化用户体验。
附图说明
图1是本发明位姿追踪方法一实施例的流程示意图；
图2是图1中步骤S5一实施例的流程示意图；
图3是图1中步骤S5另一实施例的流程示意图；
图4是图1中步骤S6一实施例的流程示意图；
图5是本发明位姿追踪***一实施例的功能框图；
图6为本发明一实施例所提供的设备的硬件结构图。
具体实施方式
由背景技术可知，目前位姿追踪的鲁棒性和用户体验有待提高。
为了解决技术问题，本发明实施例提供一种位姿追踪方法。参考图1，是本发明位姿追踪方法一实施例的流程示意图。
如图1所示，本实施例中，位姿追踪方法包括以下基本步骤：
步骤S1：获取可移动设备的当前帧图像，可移动设备上设置有多个定位灯；
步骤S2：基于当前帧图像，提取定位灯对应的光斑特征；
步骤S3：获取惯性测量数据；
步骤S4：判断位姿是否已初始化；
步骤S5：如果是，则获取前一帧图像对应的位姿作为参考位姿，并基于所述光斑特征和所述参考位姿以及所述惯性测量数据，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；
步骤S6：否则，基于光斑特征，进行初始化搜索，用于获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；
步骤S7：通过紧耦合的方式对光斑序号和惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
本发明实施例提供的位姿追踪方法，基于所述光斑特征和所述参考位姿以及所述惯性测量数据获得光斑序号，或基于光斑特征进行初始化搜索以获得光斑序号；通过紧耦合的方式对光斑序号和惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿，从而对视觉信息和惯性测量数据进行融合，惯性测量的帧率较高，有利于提高当前位姿的输出频率，而且，通过视觉信息和惯性测量数据的融合，还对当前位姿进行了平滑滤波，从而减小输出抖动；此外，惯性测量数据短时间内的精度较高，通过紧耦合的方式对光斑序号和惯性测量数据进行融合，还有利于将视觉信息和惯性测量数据相互弥补，不仅有利于缩小图像搜索范围、大幅度提升视觉跟踪的鲁棒性，而且还能够有效削除累积误差，进而提升位姿追踪方法的鲁棒性和优化用户体验。
为使本发明实施例的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
参考图1，执行步骤S1：获取可移动设备的当前帧图像，可移动设备上设置有多个定位灯。获取当前帧图像，以便后续基于当前帧图像对当前帧时刻的可移动设备的位姿进行追踪。具体地，后续基于当前帧图像，提取定位灯对应的光斑特征。相应的，图像包括定位灯对应的光斑。
其中，可移动设备为能够移动且待进行位姿追踪的设备。可移动设备上设置有多个定位灯，以便后续基于当前帧图像，提取定位灯对应的光斑特征，进而基于光斑特征获得可移动设备的位姿。
作为一种示例，可移动设备为手柄(Handheld Controller)。例如：可移动设备为应用于VR、AR或MR的手柄。作为一种示例，手柄包括灯环，灯环上设置有多个定位灯。在具体实施中，定位灯可以为LED灯。在其他实施例中，可移动设备还可以为其他带定位灯或者反光小球的移动位姿追踪设备。
在具体实施中，可移动设备与显示端配合使用，后续获得可移动设备的位姿，以便基于可移动设备的位姿，更新显示端的显示内容。例如：可移动设备为应用于VR、AR或MR的手柄，显示端为与手柄配合使用的头戴式显示设备。具体地，头戴式显示设备可以为VR、AR或MR智能眼镜。
作为一种示例，通过显示端上设置的相机，获取可移动设备的当前帧图像。
作为一种示例，相机可以为多目相机，例如：多目红外相机。相应的，本实施例中，当前帧的图像为多个相机拍摄的图像，即当前帧的图像为多个。
参考图1，执行步骤S2：基于当前帧图像，提取定位灯对应的光斑特征。
提取定位灯对应的光斑特征，以便后续能够基于光斑特征，确定光斑对应的定位灯在可移动设备上的序号作为光斑序号，进而基于光斑序号获得可移动设备的位姿。
在具体实施中，光斑特征通常为圆形或椭圆形的光斑。
作为一种示例，在提取定位灯对应的光斑特征的过程中，还计算每个光斑特征的中心点位置，以便在后续确定光斑序号时，能够基于每个光斑的中心点位置和光斑序号，来计算可移动设备在当前帧图像时刻的当前位姿。
作为一种示例，利用椭圆拟合的方法计算每个光斑特征的中心点位置。
本实施例中，基于当前帧图像，提取定位灯的光斑特征的步骤包括：基于当前帧图像，检测可移动设备框；从可移动设备框中提取定位灯的光斑特征。
更具体地，本实施例中，可移动设备为手柄；基于当前帧图像，检测手柄框，从手柄框中提取定位灯的光斑特征。
在具体实施中，可以利用视觉方法或者深度学习方法从图像中检测出手柄框，以及在每个提取出的手柄框内进行光斑特征的提取。
作为一种示例，视觉方法可以包括对当前帧图像进行二值化处理，之后将二值化处理后的图像中亮度高于预设亮度阈值的特征进行提取，作为光斑特征。
需要说明的是，本实施例中，位姿追踪方法还包括：在基于当前帧图像，提取定位灯的光斑特征之后，且在判断位姿是否已初始化之前，执行步骤S8：对光斑特征进行异常特征剔除处理，用于剔除异常的环境特征。
对光斑特征进行异常特征剔除处理，以便剔除异常的环境特征，防止异常环境特征对光斑特征产生干扰，从而提高所提取的光斑特征的准确度，进而提高后续基于光斑特征确定光斑序号的准确性。
具体地，作为示例，在实际的使用场景中，经常会遇到灯光、自然光等各种环境光照的干扰，导致检测出的光斑特征可能不是可移动设备的定位灯的光斑，对光斑特征进行异常特征剔除处理，以便对环境光干扰进行异常特征剔除。
作为一种示例，定位灯的光斑特征通常为具有一定大小的圆形或者椭圆形，而环境光干扰通常为不规则形状，通过判断特征的形状或者大小，即可剔除大部分异常特征。
继续参考图1，执行步骤S3：获取惯性测量数据。获取惯性测量数据，以便后续能够基于惯性测量数据和光斑特征以及参考位姿确定光斑序号，以及能够通过紧耦合的方式对光斑序号和惯性测量数据进行融合，以获得可移动设备在当前帧图像时刻的当前位姿。
本实施例中，惯性测量数据包括角速度信息和加速度信息。本实施例中，惯性测量数据还包括重力信息。
更具体地，在具体实施中，可移动设备中设置有惯性测量单元(InertialMeasurement Unit，IMU)，获取惯性测量单元所测量的惯性测量数据。
本实施例中，获取惯性测量数据包括：获取前一帧图像至当前帧图像之间的惯性测量数据，作为第一惯性测量数据。
后续获取前一帧图像对应的位姿作为参考位姿；本实施例中，获取前一帧图像至当前帧图像之间的惯性测量数据，以便后续对惯性测量数据进行预积分处理，获得预积分数据，进而能够基于参考位姿、以及前一帧图像至当前帧图像之间的预积分数据，获得预测位姿，以及便于后续通过紧耦合的方式对光斑序号和预积分数据进行融合，以获得可移动设备在当前帧图像时刻的当前位姿。
继续参考图1，执行步骤S4：判断位姿是否已初始化。
判断位姿是否已初始化，以便判断位姿数据是否具有先验信息，进而便于基于先验信息获得光斑序号。
继续参考图1，执行步骤S5：如果是，则获取前一帧图像对应的位姿作为参考位姿，并基于光斑特征和参考位姿以及惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号。获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号，从而确定每个定位灯在空间中的位置，相应便于获得可移动设备的位姿。
本实施例中，基于光斑特征和参考位姿及惯性测量数据，获得光斑序号，有利于利用惯性测量数据减小搜索匹配范围，提高光斑序号的准确度和匹配度。
其中，在当位姿已初始化时，即前一帧图像对应的位姿是已知的，从而获取前一帧图像对应的位姿作为参考位姿，参考位姿用于结合惯性测量数据，预测当前帧图像对应的预测位姿，还用于结合光斑特征和惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号。
结合参考图2，图2是图1中步骤S5一实施例的流程示意图。
作为一种示例，基于光斑特征和参考位姿以及惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号的步骤S5包括：
如图2所示，执行步骤S51：基于参考位姿和惯性测量数据，获得当前帧图像对应的预测位姿。获得当前帧图像对应的预测位姿，以便后续根据预测位姿，获得可移动设备上所有定位灯在图像上的投影，进而能够基于定位灯在图像上的投影，与光斑特征进行匹配，与获得投影与光斑特征最佳匹配的光斑的序号，作为光斑序号。
本实施例中，所获取的惯性测量数据为前一帧图像至当前帧图像之间的第一惯性测量数据，相应的，利用前一帧图像对应的参考位姿和第一惯性测量数据，能够较为准确地预测当前帧的预测位姿，相应在后续将可移动设备上的定位灯在图像上的投影与光斑特征进行匹配的过程中，有利于有效缩小图像搜索范围，进而提升所获得的光斑序号的准确性和视觉追踪的鲁棒性。
本实施例中，预测位姿包括预测旋转信息和预测位置信息及预测速度信息。
本实施例中，基于参考位姿和惯性测量数据，获得当前帧图像对应的预测位姿的步骤包括：对惯性测量数据进行预积分处理，获得预积分数据；基于参考位姿和预积分数据，获得当前帧图像对应的预测位姿。
本实施例中，预积分数据包括IMU预积分增量，IMU预积分增量包括：IMU预积分旋转增量、IMU预积分位置增量、以及IMU预积分速度增量。
本实施例中，参考位姿包括参考旋转信息和参考位置信息以及参考速度信息；基于参考位姿和预积分数据，获得当前帧对应的预测位姿的步骤包括：参考旋转信息右乘IMU预积分旋转增量得到预测旋转信息；参考位置信息累加IMU预积分位置增量、参考速度信息一次积分量和重力信息二次积分量得到预测位置信息；参考速度信息累加IMU预积分速度增量和重力信息一次积分量得到预测速度信息。
如图2所示，执行步骤S52：根据预测位姿，获得可移动设备上所有定位灯在图像上的投影。获得所有定位灯在图像上的投影，以便后续将投影与光斑特征进行匹配，以获得最佳匹配的光斑的序号，作为光斑序号。
具体地，本实施例中，当前帧的图像为多个相机所拍摄的多张图像，相应的，获得所有定位灯在各个相机对应的当前帧图像上的投影，以便分别将投影与对应的图像上的光斑特征进行匹配。
如图2所示，执行步骤S53：将可移动设备上所有定位灯在图像上的投影，与光斑特征进行匹配，获得投影与光斑特征最佳匹配的光斑的序号，作为光斑序号。
在具体实施中，可以通过将投影与光斑特征之间的大小、形状及位置进行最近邻搜索(Nearest Neighbor Search，NNS)和比较，并判断光斑匹配数量和重投影误差是否满足预设要求，以获得投影与光斑特征最佳匹配的光斑的序号。
结合参考图3，是图1中步骤S5另一实施例的流程示意图。
作为另一实施例，基于光斑特征和初始位姿以及惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号的步骤S5包括：
如图3所示，执行步骤S511：基于参考位姿和惯性测量数据，获得当前帧图像对应的预测位姿，预测位姿包括预测旋转信息。
后续利用预测旋转信息，获得可移动设备的初始位置信息。本实施例中，预测位姿还包括预测位置信息。
本实施例中，基于参考位姿和惯性测量数据，获得当前帧图像对应的预测位姿的步骤包括：对惯性测量数据进行预积分处理，获得预积分数据；基于参考位姿和预积分数据，获得当前帧图像对应的预测位姿。
对基于参考位姿和惯性测量数据获得当前帧图像对应的预测位姿的详细描述，可结合参考前述对步骤S51的详细描述，本实施例在此不再赘述。
如图3所示，执行步骤S512：进行一次或多次迭代的跟踪匹配处理。
其中，每次跟踪匹配处理包括：步骤S513：随机选取两个光斑特征，作为随机光斑特征；步骤S514：估计随机光斑特征对应的最佳光斑序号；步骤S515：基于最佳光斑序号和预测旋转信息，获得可移动设备的初始位置信息；步骤S516：基于预测旋转信息和初始位置信息，获得所有定位灯在图像中的投影；步骤S517：将投影与光斑特征进行匹配，判断匹配数量和重投影误差是否满足匹配要求；步骤S518：如果是，则判断匹配成功，并输出当前的光斑序号；否则，重复进行跟踪匹配处理。
本实施例中，通过进行一次或多次迭代的跟踪匹配处理，采用两点RANSAC(RandomSample Consensus)的方法进行跟踪匹配处理，其中，基于随机光斑特征对应的最佳光斑序号以及预测旋转信息，有利于获得准确度较高的初始位置信息，相应有利于提高在将投影与光斑特征进行匹配的匹配度，而且，因为无需预测位姿中的预测位置信息进行跟踪匹配，还有利于容忍较大的预测位置信息误差，从而大幅度提高跟踪匹配的鲁棒性。
更具体地，在基于参考位姿和惯性测量数据，获得当前帧图像对应的预测位姿的过程中，通常需要对惯性测量数据进行预积分，其中，预测旋转信息通常仅需要经过一次积分，预测位置信息通常至少需要进行两次积分，因此，基于惯性测量数据和参考位姿所获得的预测旋转信息的准确度，相较于基于惯性测量数据和参考位姿所获得的预测位置信息的准确度更高，也就是说，预测位置信息的误差更大，尤其是随着时间的推移，积分误差积累的概率更高，预测位置信息的误差也更大。本实施例中，仅利用预测旋转信息进行跟踪匹配处理，从而进一步提高跟踪匹配的鲁棒性，并且能够容忍较大的预测位置信息的误差。
作为一种示例，估计随机光斑特征对应的最佳光斑序号的步骤包括：根据预测位姿，获得可移动设备上的定位灯在图像上的投影，作为投影光斑；在投影光斑附近进行搜索匹配，获得与随机光斑匹配的投影光斑的序号，作为最佳光斑序号。
作为一种示例，基于最佳光斑序号和预测旋转信息，获得可移动设备的初始位置信息的方法包括PnP(Perspective-n-Point)算法。
本实施例中，基于随机光斑特征对应的最佳光斑序号以及预测旋转信息，有利于获得准确度较高的初始位置信息，相应在基于预测旋转信息和初始位置信息，获得所有定位灯在图像中的投影，以及将投影与光斑特征进行匹配的过程中，更容易找到与投影匹配度高的光斑。
其中，判断匹配数量和重投影误差是否满足匹配要求包括：判断匹配的数量至少大于预设数量，或者，匹配的数量占总数量的占比至少大于预设比值；以及，判断重投影误差是否在预设阈值范围内。
继续参考图1，执行步骤S6：否则，基于光斑特征，进行初始化搜索，用于获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号。
结合参考图4，是图1中步骤S6一实施例的流程示意图。
如图4所示，本实施例中，基于光斑特征，进行初始化搜索的步骤S6包括：执行步骤S61：对光斑特征进行排序；步骤S62：获得最长的连续光斑序列；步骤S63：对连续光斑序列的起始点进行遍历搜索；步骤S64：根据当前连续光斑序列所对应的光斑序号，计算可移动设备的位姿信息；步骤S65：基于位姿信息，将当前连续光斑序列对应的定位灯投影至当前帧图像中，并计算重投影误差；步骤S66：获得重投影误差满足预设阈值要求的最佳连续光斑序列；步骤S67：基于最佳连续光斑序列，确定光斑对应的定位灯在可移动设备上的序号，作为光斑序号。
本实施例中，利用定位灯之间的连续位置关系，进行初始化搜索，以确定光斑对应的定位灯的序号，有利于缩小搜索范围，进而大幅度缩短初始化搜索的时间。
本实施例中，对光斑特征进行排序，以获得最长的连续光斑序列，从而便于后续基于连续光斑序列中，光斑特征之间的连续位置关系，对序列起始点进行遍历搜索，以减小搜索范围。
本实施例中，在对光斑特征进行排序，并获得最长的连续光斑序列的过程中，连续光斑序列的长度至少为预设长度，从而保证能够通过PnP求解出位姿，并且减少错误的初始化。
作为一种示例，可移动设备为手柄，以手柄上的定位灯的数量为14个为示例进行说明，在对光斑特征进行排序以及获得最长的连续光斑序列的过程中，连续光斑序列的长度至少为5个点，从而保证能够通过PnP求解出位姿，并且减少错误的初始化。
在获得最长的连续光斑序列后，对连续光斑序列的起始点进行遍历搜索，以便基于当前序列对应的光斑序号，计算位姿信息，进而便于基于位姿信息计算重投影误差，以找到重投影误差满足预设阈值要求的最佳连续光斑序列。
作为一种示例，可移动设备上的定位灯的数量为14个，对连续光斑序列的起始点从0-13进行遍历搜索。例如：序列起始点的序号为i，则连续光斑序列的光斑序号依次为：i、i+1、i+2、…。
基于当前连续光斑序列对应的光斑序号，计算位姿信息。作为一种示例，根据当前连续光斑序列所对应的光斑序号，计算可移动设备的位姿信息的方法包括PnP算法。
随后，将重投影误差满足预设阈值要求的连续光斑序列，作为最佳连续光斑序列，并输出序列对应的光斑序号，以作为光斑序号。
参考图1，执行步骤S7：通过紧耦合(Tightly Coupled)的方式对光斑序号和惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
本实施例中，通过紧耦合的方式对光斑序号和惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿，从而对视觉信息和惯性测量数据进行融合，惯性测量的帧率较高，有利于提高当前位姿的输出频率，而且，通过视觉信息和惯性测量数据的融合，还对当前位姿进行了平滑滤波，从而减小输出抖动。
此外，惯性测量数据短时间内的精度较高，通过紧耦合的方式对光斑序号和惯性测量数据进行融合，还有利于将视觉信息和惯性测量数据相互弥补，不仅有利于缩小图像搜索范围、大幅度提升视觉跟踪的鲁棒性，而且还能够有效削除累积误差，进而提升位姿追踪方法的鲁棒性和优化用户体验。
作为示例，通过紧耦合的方式对光斑序号和惯性测量数据进行融合的步骤包括：对惯性测量数据进行预积分，获得预积分数据；通过紧耦合的方式对光斑序号和预积分数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
具体地，本实施例中，利用平方根卡尔曼滤波器(Square Root Kalman Filter)，通过紧耦合的方式对光斑序号和惯性测量数据进行融合，获得可移动设备的当前位姿。
标准的扩展卡尔曼滤波器(Extended Kalman Filter，EKF)的数值稳定性较低，且在实际使用的过程中，容易发生状态发生发散的情况，和标准的扩展卡尔曼滤波器相比而言，平方根滤波器的数值稳定，且利用单精度浮点数(float)精度计算结果可以达到双精度浮点数(double)同等精度，因此采用平方根滤波器在提升数值稳定性的同时，还能够减少计算量。
具体地，平方根滤波器包括状态方程和观测方程。
其中，本实施例中，k-1时刻至k时刻的状态方程如下：
其中，状态估计量包括：角度误差ψk、位置误差Δpk、速度误差Δvk、陀螺零偏(Bias)bgk、加速度计零偏bak；惯性测量数据的预积分增量包括：旋转增量位置增量速度增量/>
惯性测量数据的预积分增量对零偏的雅可比矩阵(Jacobi Matrix)包括：旋转增量对陀螺零偏的雅可比矩阵JRg、位置增量对陀螺零偏的雅可比矩阵JPg、位置增量对加速度计零偏的雅可比矩阵JPa、速度增量对陀螺零偏的雅可比矩阵JVg、速度增量对加速度计零偏的雅可比矩阵JVa；状态传递噪声nk-1。
本实施例中，观测方程如下：
其中：z(1)，z(2)，...，z(n)为n个光斑特征的重投影误差；为第j个光斑特征的重投影误差的雅可比矩阵，νk为观测噪声。
本实施例中，基于状态方程和观测方程，对光斑序号和惯性测量数据进行紧耦合方式的融合，以获得可移动设备在当前帧图像时刻的当前位姿。
继续参考图1，本实施例中，位姿追踪方法还包括：在获得可移动设备在当前帧图像时刻的当前位姿后，执行步骤S9：获取当前帧图像至显示渲染时刻之间的惯性测量数据，作为第二惯性测量数据；基于第二惯性测量数据和当前位姿，获得显示渲染时刻的可移动设备位姿。
基于第二惯性测量数据和当前位姿，获得显示渲染时刻的可移动设备位姿，从而有效减少输出位姿的延迟，提高用户感受度。
具体地，通常图像的帧率较低，通常具有较大的延时；惯性测量数据的帧率较高，实时性较好，通过结合第二惯性测量数据和当前位姿，以获得显示渲染时刻的可移动设备位姿，有利于大幅度减少输出延迟。
并且，由前述记载可知，本实施例中获得的当前位姿的精度高、位姿追踪的鲁棒性较好，从而基于当前位姿和惯性测量数据获得的显示渲染时刻的可移动设备位姿的准确度较高。
此外，在具体实施中，当前帧图像与显示渲染时刻之间的时间间隔短，基于当前位姿、以及当前帧图像至显示渲染时刻之间的惯性测量数据所获得的可移动设备的位姿的误差小、精度高。
在具体实施中，可移动设备与显示端配合使用，获得可移动设备的位姿，以便基于可移动设备的位姿，更新显示端的显示内容。本实施例中，显示渲染时刻指的是在显示端的显示内容呈现的时刻。
继续参考图1，本实施例中，位姿追踪方法还包括：在获得显示渲染时刻的可移动设备位姿之后，执行步骤S10：对可移动设备位姿的数据进行平滑滤波处理。对可移动设备位姿的数据进行平滑滤波处理，以消除轨迹抖动，获得平滑的位姿轨迹。
作为一种示例，在具体实施中，可以采用低通滤波器(Low-pass filter)等方式，进行平滑滤波处理。在其他实施例中，还可以采用其他的方式，进行平滑滤波处理。
相应的，本发明还提供一种位姿追踪***。图5是本发明手柄追踪***一实施例的功能框图。
参考图5，本实施例中，手柄追踪***100包括：图像处理模块101，用于获取可移动设备的当前帧图像，可移动设备上设置有多个定位灯；图像处理模块101还用于基于当前帧图像，提取定位灯的光斑特征；惯性测量模块102，用于获取惯性测量数据；初始化判断模块103，用于判断位姿是否已初始化；跟踪匹配模块104，用于在判断位姿已初始化时，获取前一帧图像对应的位姿作为参考位姿，并基于所述光斑特征和所述参考位姿以及所述惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；初始化搜索模块105，用于在判断位姿未初始化时，基于光斑特征进行初始化搜索，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；视觉惯性融合模块106，用于通过紧耦合的方式对光斑序号和惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
位姿追踪***100中，跟踪匹配模块104基于光斑特征和参考位姿以及惯性测量数据获得光斑序号，或初始化搜索模块105基于光斑特征进行初始化搜索以获得光斑序号；视觉惯性融合模块106通过紧耦合的方式对光斑序号和惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿，从而对视觉信息和惯性测量数据进行融合，惯性测量的帧率较高，有利于提高当前位姿的输出频率，而且，通过视觉信息和惯性测量数据的融合，还对当前位姿进行了平滑滤波，从而减小输出抖动；此外，惯性测量数据短时间内的精度较高，通过紧耦合的方式对光斑序号和惯性测量数据进行融合，还有利于将视觉信息和惯性测量数据相互弥补，不仅有利于缩小图像搜索范围、大幅度提升视觉跟踪的鲁棒性，而且还能够有效削除累积误差，进而提升位姿追踪的鲁棒性和优化用户体验。
图像处理模块101用于获取当前帧图像，以便基于当前帧图像对当前帧时刻的可移动设备的位姿进行追踪。具体地，图像处理模块101还用于基于当前帧图像，提取定位灯对应的光斑特征。
相应的，图像包括定位灯对应的光斑。
其中，可移动设备为能够移动且待进行位姿追踪的设备。可移动设备上设置有多个定位灯，以便后续基于当前帧图像，提取定位灯对应的光斑特征，进而基于光斑特征获得可移动设备的位姿。
作为一种示例，可移动设备为手柄(Handheld Controller)。例如：可移动设备为应用于VR、AR或MR的手柄。作为一种示例，手柄包括灯环，灯环上设置有多个定位灯。在具体实施中，定位灯可以为LED灯。在其他实施例中，可移动设备还可以为其他带定位灯或者反光小球的移动位姿追踪设备。
在具体实施中，可移动设备与显示端配合使用，后续获得可移动设备的位姿，以便基于可移动设备的位姿，更新显示端的显示内容。例如：可移动设备为应用于VR、AR或MR的手柄，显示端为与手柄配合使用的头戴式显示设备。具体地，头戴式显示设备可以为VR、AR或MR智能眼镜。
作为一种示例，通过显示端上设置的相机，获取可移动设备的当前帧图像。
作为一种示例，相机可以为多目相机，例如：多目红外相机。相应的，本实施例中，当前帧的图像为多个相机拍摄的图像，即当前帧的图像为多个。
图像处理模块101还用于基于当前帧图像，提取定位灯对应的光斑特征，以便基于光斑特征，确定光斑对应的定位灯在可移动设备上的序号作为光斑序号，进而基于光斑序号获得可移动设备的位姿。
在具体实施中，光斑特征通常为圆形或椭圆形的光斑。
作为一种示例，图像处理模块101还计算每个光斑特征的中心点位置，以便在确定光斑序号时，能够基于每个光斑的中心点位置和光斑序号，来计算可移动设备在当前帧图像时刻的当前位姿。
作为一种示例，利用椭圆拟合的方法计算每个光斑特征的中心点位置。
本实施例中，图像处理模块101用于基于当前帧图像，检测可移动设备框；还用于从可移动设备框中提取定位灯的光斑特征。更具体地，本实施例中，可移动设备为手柄；基于当前帧图像，检测手柄框，从手柄框中提取定位灯的光斑特征。
在具体实施中，可以利用视觉方法或者深度学习方法从图像中检测出手柄框，以及在每个提取出的手柄框内进行光斑特征的提取。
作为一种示例，视觉方法可以包括对当前帧图像进行二值化处理，之后将二值化处理后的图像中亮度高于预设亮度阈值的特征进行提取，作为光斑特征。
需要说明的是，本实施例中，图像处理模块101还用于对光斑特征进行异常特征剔除处理，用于剔除异常的环境特征，以防止异常环境特征对光斑特征产生干扰，从而提高所提取的光斑特征的准确度，进而提高基于光斑特征确定光斑序号的准确性。
具体地，作为一种示例，在实际的使用场景中，经常会遇到灯光、自然光等各种环境光照的干扰，导致检测出的光斑特征可能不是可移动设备的定位灯的光斑，对光斑特征进行异常特征剔除处理，以便对环境光干扰进行异常特征剔除。
作为一种示例，定位灯的光斑特征通常为具有一定大小的圆形或者椭圆形，而环境光干扰通常为不规则形状，通过判断特征的形状或者大小便能够剔除大部分异常特征。
惯性测量模块102用于获取惯性测量数据，以便跟踪匹配模块104能够基于惯性测量数据和光斑特征以及参考位姿确定光斑序号，还便于视觉惯性融合模块106能够通过紧耦合的方式对光斑序号和惯性测量数据进行融合，以获得可移动设备在当前帧图像时刻的当前位姿。
本实施例中，惯性测量数据包括角速度信息和加速度信息。本实施例中，惯性测量数据还包括重力信息。更具体地，在具体实施中，可移动设备中设置有惯性测量单元(Inertial Measurement Unit，IMU)，获取惯性测量单元所测量的惯性测量数据。
本实施例中，获取惯性测量数据包括：获取前一帧图像至当前帧图像之间的惯性测量数据，作为第一惯性测量数据。
本实施例中，还获取前一帧图像对应的位姿作为参考位姿；惯性测量模块102获取前一帧图像至当前帧图像之间的惯性测量数据，以便对惯性测量数据进行预积分处理，获得预积分数据，进而能够基于参考位姿、以及前一帧图像至当前帧图像之间的预积分数据，获得预测位姿，以及便于视觉惯性融合模块106通过紧耦合的方式对光斑序号和预积分数据进行融合，以获得可移动设备在当前帧图像时刻的当前位姿。
初始化模块103用于判断位姿是否已初始化，以便判断位姿数据是否具有先验信息，进而便于基于先验信息获得光斑序号。
跟踪匹配模块104用于在判断位姿已初始化时，获取前一帧图像对应的位姿作为参考位姿，并基于所述光斑特征和所述参考位姿以及所述惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号。
获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号，从而确定每个定位灯在空间中的位置，相应便于获得可移动设备的位姿。
本实施例中，跟踪匹配模块104基于光斑特征和参考位姿以及惯性测量数据，获得光斑序号，有利于利用惯性测量数据以减小搜索匹配范围，提高光斑序号的准确度和匹配度。
其中，在当位姿已初始化时，即前一帧图像对应的位姿是已知的，从而获取前一帧图像对应的位姿作为参考位姿，参考位姿用于结合惯性测量数据，预测当前帧图像对应的预测位姿，还用于结合光斑特征和惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号。
作为一种示例，跟踪匹配模块104包括：预测位姿获取单元(图未示)，用于基于参考位姿和惯性测量数据，获得当前帧图像对应的预测位姿；匹配单元(图未示)，用于根据预测位姿，获得可移动设备上所有定位灯在图像上的投影，并将投影与光斑特征进行匹配，获得投影与光斑特征最佳匹配的光斑的序号，作为光斑序号。
预测位姿获取单元获得当前帧图像对应的预测位姿，以便匹配单元根据预测位姿，获得可移动设备上所有定位灯在图像上的投影，进而能够基于定位灯在图像上的投影，与光斑特征进行匹配，与获得投影与光斑特征最佳匹配的光斑的序号，作为光斑序号。
本实施例中，所获取的惯性测量数据为前一帧图像至当前帧图像之间的第一惯性测量数据，相应的，利用前一帧图像对应的参考位姿和第一惯性测量数据，能够较为准确地预测当前帧的预测位姿，相应在匹配单元将定位灯在图像上的投影与光斑特征进行匹配的过程中，有利于有效缩小图像搜索范围，进而提升所获得的光斑序号的准确性和视觉追踪的鲁棒性。
本实施例中，预测位姿包括预测旋转信息和预测位置信息及预测速度信息。具体地，本实施例中，预测位姿获取单元用于对惯性测量数据进行预积分处理，获得预积分数据；还用于基于参考位姿和预积分数据，获得当前帧图像对应的预测位姿。
本实施例中，预积分数据包括IMU预积分增量，IMU预积分增量包括：IMU预积分旋转增量、IMU预积分位置增量、以及IMU预积分速度增量。
本实施例中，参考位姿包括参考旋转信息和参考位置信息以及参考速度信息；基于参考位姿和预积分数据，获得当前帧对应的预测位姿的步骤包括：参考旋转信息右乘IMU预积分旋转增量得到预测旋转信息；参考位置信息累加IMU预积分位置增量、参考速度信息一次积分量和重力信息二次积分量得到预测位置信息；参考速度信息累加IMU预积分速度增量和重力信息一次积分量得到预测速度信息。
匹配单元用于根据预测位姿，获得可移动设备上所有定位灯在图像上的投影，以便将投影与光斑特征进行匹配，以获得最佳匹配的光斑的序号，作为光斑序号。
具体地，本实施例中，当前帧的图像为多个相机所拍摄的多张图像，相应的，获得所有定位灯在各个相机对应的当前帧图像上的投影，以便分别将投影与对应的图像上的光斑特征进行匹配。
在具体实施中，匹配单元可以通过将投影与光斑特征之间的大小、形状以及位置进行最近邻搜索和比较，并判断光斑匹配数量和重投影误差是否满足预设要求，以获得投影与光斑特征最佳匹配的光斑的序号。
作为另一实施例，跟踪匹配模块104包括：预测位姿获取单元(图未示)，用于基于参考位姿和惯性测量数据，获得当前帧图像对应的预测位姿，预测位姿包括预测旋转信息；匹配单元(图未示)，用于进行一次或多次迭代的跟踪匹配处理；其中，每次跟踪匹配处理包括：随机选取两个光斑特征，作为随机光斑特征；估计随机光斑特征对应的最佳光斑序号；基于最佳光斑序号和预测旋转信息，获得可移动设备的初始位置信息；基于预测旋转信息和初始位置信息，获得所有定位灯在图像中的投影；将投影与光斑特征进行匹配，判断匹配数量和重投影误差是否满足匹配要求；如果是，则判断匹配成功，并输出当前的光斑序号；否则，重复进行跟踪匹配处理。
预测位姿获取单元获得当前帧图像对应的预测位姿，预测位姿包括预测旋转信息，以便利用预测旋转信息，获得可移动设备的初始位置信息。本实施例中，预测位姿还包括预测位置信息。
具体地，本实施例中，预测位姿获取单元用于对惯性测量数据进行预积分处理，获得预积分数据；预测位姿获取单元还用于基于参考位姿和预积分数据，获得当前帧图像对应的预测位姿。
对基于参考位姿和惯性测量数据获得当前帧图像对应的预测位姿的详细描述，可结合参考前述详细描述，本实施例在此不再赘述。
本实施例中，匹配单元进行一次或多次迭代的跟踪匹配处理，采用两点RANSAC(Random Sample Consensus)的方法进行跟踪匹配处理，其中，基于随机光斑特征对应的最佳光斑序号以及预测旋转信息，有利于获得准确度较高的初始位置信息，相应有利于提高在将投影与光斑特征进行匹配的匹配度，而且，因为无需预测位姿中的预测位置信息进行跟踪匹配，还有利于容忍较大的预测位置信息误差，从而大幅度提高跟踪匹配的鲁棒性。
更具体地，在预测位姿获取单元基于参考位姿和惯性测量数据，获得当前帧图像对应的预测位姿的过程中，通常需要对惯性测量数据进行预积分，其中，预测旋转信息通常仅需要经过一次积分，预测位置信息通常至少需要进行两次积分，因此，基于惯性测量数据和参考位姿所获得的预测旋转信息的准确度，相较于基于惯性测量数据和参考位姿所获得的预测位置信息的准确度更高，也就是说，预测位置信息的误差更大，尤其是随着时间的推移，积分误差积累的概率更高，预测位置信息的误差也更大。本实施例中，匹配单元仅利用预测旋转信息进行跟踪匹配处理，从而进一步提高跟踪匹配的鲁棒性，并且能够容忍较大的预测位置信息的误差。
作为一种示例，估计随机光斑特征对应的最佳光斑序号包括：根据预测位姿，获得可移动设备上定位灯在图像上的投影，作为投影光斑；在投影光斑附近进行搜索匹配，获得与随机光斑匹配的投影光斑的序号，作为最佳光斑序号。
作为一种示例，基于最佳光斑序号和预测旋转信息，获得可移动设备的初始位置信息的方法包括PnP(Perspective-n-Point)算法。
本实施例中，基于随机光斑特征对应的最佳光斑序号以及预测旋转信息，有利于获得准确度较高的初始位置信息，相应在基于预测旋转信息和初始位置信息，获得所有定位灯在图像中的投影，以及将投影与光斑特征进行匹配的过程中，更容易找到与投影匹配度高的光斑。
其中，判断匹配数量和重投影误差是否满足匹配要求包括：判断匹配的数量至少大于预设数量，或者，匹配的数量占总数量的占比至少大于预设比值；判断重投影误差是否在预设阈值范围内。
初始化搜索模块105，用于在判断未进行初始化时，基于光斑特征，进行初始化搜索，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号。
作为一种示例，初始化搜索模块包括：序列获取单元(图未示)，用于对光斑特征进行排序，以获得最长的连续光斑序列；遍历搜索单元(图未示)，用于对连续光斑序列的起始点进行遍历搜索；位姿计算单元(图未示)，用于根据当前连续光斑序列所对应的光斑序号，计算可移动设备的位姿信息；重投影误差计算单元(图未示)，用于基于位姿信息，将当前连续光斑序列对应的定位灯投影至当前帧图像中，并计算重投影误差；序号确定单元(图未示)，用于获得重投影误差满足预设阈值要求的最佳连续光斑序列，并基于最佳连续光斑序列，确定光斑对应的定位灯在可移动设备上的序号，作为光斑序号。
本实施例中，初始化搜索模块105利用定位灯之间的连续位置关系，进行初始化搜索，以确定光斑对应的定位灯的序号，有利于缩小搜索范围，进而大幅度缩短初始化搜索的时间。
本实施例中，序列获取单元对光斑特征进行排序，以获得最长的连续光斑序列，从而便于基于连续光斑序列中，光斑特征之间的连续位置关系，对序列起始点进行遍历搜索，以减小搜索范围。
本实施例中，序列获取单元在对光斑特征进行排序，并获得最长的连续光斑序列的过程中，连续光斑序列的长度至少为预设长度，从而保证能够通过PnP求解出位姿，并且减少错误的初始化。
作为一种示例，可移动设备为手柄，以手柄上的定位灯的数量为14个为示例进行说明，在对光斑特征进行排序以及获得最长的连续光斑序列的过程中，连续光斑序列的长度至少为5个点，从而保证能够通过PnP求解出位姿，并且减少错误的初始化。
遍历搜索单元对连续光斑序列的起始点进行遍历搜索，以便位姿计算单元基于当前序列对应的光斑序号，计算位姿信息，进而便于重投影误差计算单元基于位姿信息计算重投影误差，以使得序号确定单元找到重投影误差满足预设阈值要求的最佳连续光斑序列。
作为一种示例，可移动设备上的定位灯的数量为14个，对连续光斑序列的起始点从0-13进行遍历搜索。例如：序列起始点的序号为i，则连续光斑序列的光斑序号依次为：i、i+1、i+2、…。
位姿计算单元基于当前连续光斑序列对应的光斑序号，计算位姿信息。作为一种示例，根据当前连续光斑序列所对应的光斑序号，计算可移动设备的位姿信息的方法包括PnP算法。
随后，将重投影误差满足预设阈值要求的连续光斑序列，作为最佳连续光斑序列，并输出序列对应的光斑序号，以作为光斑序号。
本实施例中，视觉惯性融合模块106通过紧耦合(Tightly Coupled)的方式对光斑序号和惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿，从而对视觉信息和惯性测量数据进行融合，惯性测量的帧率较高，有利于提高当前位姿的输出频率，而且，通过视觉信息和惯性测量数据的融合，还对当前位姿进行了平滑滤波，从而减小输出抖动。
此外，惯性测量数据短时间内的精度较高，视觉惯性融合模块106通过紧耦合的方式对光斑序号和惯性测量数据进行融合，还有利于将视觉信息和惯性测量数据相互弥补，不仅有利于缩小图像搜索范围、大幅度提升视觉跟踪的鲁棒性，而且还能够有效削除累积误差，进而提升位姿追踪***100的鲁棒性和优化用户体验。
作为一种示例，视觉惯性融合模块106用于对惯性测量数据进行预积分，获得预积分数据；还用于通过紧耦合的方式对光斑序号和预积分数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
具体地，本实施例中，视觉惯性融合模块106利用平方根卡尔曼滤波器(SquareRoot Kalman Filter)，通过紧耦合的方式对光斑序号和惯性测量数据进行融合，获得可移动设备的当前位姿。
标准的扩展卡尔曼滤波器(Extended Kalman Filter，EKF)的数值稳定性较低，且在实际使用的过程中，容易发生状态发生发散的情况，和标准的扩展卡尔曼滤波器相比而言，平方根滤波器的数值稳定，且利用单精度浮点数(float)精度计算结果可以达到双精度浮点数(double)同等精度，因此采用平方根滤波器在提升数值稳定性的同时，还能够减少计算量。
具体地，平方根滤波器包括状态方程和观测方程。
其中，本实施例中，k-1时刻至k时刻的状态方程如下：
其中，状态估计量包括：角度误差ψk、位置误差Δpk、速度误差Δvk、陀螺零偏(Bias)bgk、加速度计零偏bak；惯性测量数据的预积分增量包括：旋转增量位置增量速度增量/>
惯性测量数据的预积分增量对零偏的雅可比矩阵(Jacobi Matrix)包括：旋转增量对陀螺零偏的雅可比矩阵JRg、位置增量对陀螺零偏的雅可比矩阵JPg、位置增量对加速度计零偏的雅可比矩阵JPa、速度增量对陀螺零偏的雅可比矩阵JVg、速度增量对加速度计零偏的雅可比矩阵JVa；状态传递噪声nk-1。
本实施例中，观测方程如下：
其中：z(1)，z(2)，...，z(n)为n个光斑特征的重投影误差；为第j个光斑特征的重投影误差的雅可比矩阵，vk为观测噪声。
本实施例中，基于状态方程和观测方程，对光斑序号和惯性测量数据进行紧耦合方式的融合，以获得可移动设备在当前帧图像时刻的当前位姿。
继续参考图5，本实施例中，位姿追踪***100还包括：预测模块107，用于获取当前帧图像至显示渲染时刻之间的惯性测量数据，作为第二惯性测量数据；还用于基于第二惯性测量数据和当前位姿，获得显示渲染时刻的可移动设备位姿。
预测模块107，用于基于第二惯性测量数据和当前位姿，获得显示渲染时刻的可移动设备位姿，从而有效减少输出位姿的延迟，提高用户感受度。
具体地，通常图像的帧率较低，通常具有较大的延时；惯性测量数据的帧率较高，实时性较好，通过结合第二惯性测量数据和当前位姿，以获得显示渲染时刻的可移动设备位姿，有利于大幅度减少输出延迟。
并且，由前述记载可知，本实施例中视觉惯性融合模块106输出的当前位姿的精度高、位姿追踪的鲁棒性较好，从而预测模块107基于当前位姿和惯性测量数据获得的显示渲染时刻的可移动设备位姿的准确度较高。
此外，在具体实施中，当前帧图像与显示渲染时刻之间的时间间隔短，基于当前位姿、以及当前帧图像至显示渲染时刻之间的惯性测量数据所获得的可移动设备的位姿的误差小、精度高。
在具体实施中，在位姿追踪***100中，可移动设备与显示端配合使用，获得可移动设备的位姿，以便基于可移动设备的位姿，更新显示端的显示内容。本实施例中，显示渲染时刻指的是在显示端的显示内容呈现的时刻。
继续参考图5，本实施例中，位姿追踪***100还包括：平滑滤波模块108，用于对可移动设备位姿的数据进行平滑滤波处理，以消除轨迹抖动，获得平滑的位姿轨迹。
作为一种示例，在具体实施中，平滑滤波模块108可以采用低通滤波器等，进行平滑滤波处理。在其他实施例中，还可以采用其他的方式，进行平滑滤波处理。
为了解决问题，本发明实施例还提供一种设备，该设备可以通过装载程序形式的上述位姿追踪方法，以实现本发明实施例提供的位姿追踪方法。
本实施例中，设备包括显示端和可移动设备。其中，可移动设备为能够移动且待进行位姿追踪的设备。在具体实施中，可移动设备与显示端配合使用，获得可移动设备的位姿，以便基于可移动设备的位姿，更新显示端的显示内容。
例如：可移动设备为应用于VR、AR或MR的手柄，显示端为与手柄配合使用的头戴式显示设备。具体地，作为一种示例，头戴式显示设备可以为VR、AR或MR智能眼镜。
相应的，作为一种示例，本实施例提供的设备为包含有手柄的头戴式一体机。例如：设备为头戴式6DoF一体机等。
本发明实施例提供的设备的一种可选硬件结构可以如图6所示，包括：至少一个处理器01，至少一个通信接口02，至少一个存储器03和至少一个通信总线04。
在本发明实施例中，处理器01、通信接口02、存储器03、通信总线04的数量为至少一个，且处理器01、通信接口02、存储器03通过通信总线04完成相互间的通信。
可选的，通信接口02可以为用于进行网络通信的通信模块的接口，如GSM模块的接口。
可选的，处理器01可能是中央处理器CPU，或者是特定集成电路ASIC(ApplicationSpecific Integrated Circuit)，或者是被配置成实施本发明实施例的一个或多个集成电路。
可选的，存储器03可能包含高速RAM存储器，也可能还包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。
其中，存储器03存储一条或多条计算机指令，一条或多条计算机指令被处理器01执行以实现本发明实施例提供的位姿追踪方法。
需要说明的是，上述的实现终端设备还可以包括与本发明实施例公开内容可能并不是必需的其他器件(未示出)；鉴于这些其他器件对于理解本发明实施例公开内容可能并不是必需，本发明实施例对此不进行逐一介绍。
相应地，本发明实施例还提供一种存储介质，存储介质存储有一条或多条计算机指令，一条或多条计算机指令用于实现本发明实施例的位姿追踪方法。
存储介质为计算机可读存储介质，存储介质可以为只读存储器(Read-OnlyMemory，ROM)、随机存取存储器(Random Access Memory，RAM)、U盘、移动硬盘、磁盘或光盘等各种可以存储程序代码的介质。
上述本发明的实施方式是本发明的元件和特征的组合。除非另外提及，否则元件或特征可被视为选择性的。各个元件或特征可在不与其它元件或特征组合的情况下实践。另外，本发明的实施方式可通过组合部分元件和/或特征来构造。本发明的实施方式中所描述的操作顺序可重新排列。任一实施方式的一些构造可被包括在另一实施方式中，并且可用另一实施方式的对应构造代替。对于本领域技术人员而言明显的是，所附权利要求中彼此没有明确引用关系的权利要求可组合成本发明的实施方式，或者可在提交本申请之后的修改中作为新的权利要求包括。
本发明的实施方式可通过例如硬件、固件、软件或其组合的各种手段来实现。在硬件配置方式中，根据本发明示例性实施方式的方法可通过一个或更多个专用集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理器件(DSPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器等来实现。
在固件或软件配置方式中，本发明的实施方式可以模块、过程、功能等形式实现。软件代码可存储在存储器单元中并由处理器执行。存储器单元位于处理器的内部或外部，并可经由各种己知手段向处理器发送数据以及从处理器接收数据。
虽然本发明披露如上，但本发明并非限定于此。任何本领域技术人员，在不脱离本发明的精神和范围内，均可作各种更动与修改，因此本发明的保护范围应当以权利要求所限定的范围为准。

Claims (19)

1.一种位姿追踪方法，其特征在于，包括：
获取可移动设备的当前帧图像，所述可移动设备上设置有多个定位灯；
基于所述当前帧图像，提取定位灯对应的光斑特征；
获取惯性测量数据；
判断位姿是否已初始化；
如果是，则获取前一帧图像对应的位姿作为参考位姿，并基于所述光斑特征和所述参考位姿以及所述惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；
否则，基于所述光斑特征，进行初始化搜索，用于获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；
通过紧耦合的方式对所述光斑序号和所述惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
2.如权利要求1所述的位姿追踪方法，其特征在于，所述基于所述光斑特征，进行初始化搜索的步骤包括：
对所述光斑特征进行排序；
获得最长的连续光斑序列；
对所述连续光斑序列的起始点进行遍历搜索；
根据当前连续光斑序列所对应的光斑序号，计算可移动设备的位姿信息；
基于所述位姿信息，将当前连续光斑序列对应的定位灯投影至所述当前帧图像中，并计算重投影误差；
获得重投影误差满足预设阈值要求的最佳连续光斑序列；
基于所述最佳连续光斑序列，确定光斑对应的定位灯在可移动设备上的序号，作为所述光斑序号。
3.如权利要求2所述的位姿追踪方法，其特征在于，所述根据当前连续光斑序列所对应的光斑序号，计算可移动设备的位姿信息的方法包括PnP算法。
4.如权利要求1所述的位姿追踪方法，其特征在于，所述基于所述光斑特征和所述参考位姿以及所述惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号的步骤包括：
基于参考位姿和所述惯性测量数据，获得当前帧图像对应的预测位姿；
根据所述预测位姿，获得可移动设备上所有定位灯在图像上的投影；
将所述定位灯在图像上的投影，与所述光斑特征进行匹配，获得投影与所述光斑特征最佳匹配的光斑的序号，作为所述光斑序号。
5.如权利要求4所述的位姿追踪方法，其特征在于，所述基于参考位姿和所述惯性测量数据，获得当前帧图像对应的预测位姿的步骤包括：对所述惯性测量数据进行预积分处理，获得预积分数据；基于所述参考位姿和所述预积分数据，获得当前帧图像对应的预测位姿。
6.如权利要求1所述的位姿追踪方法，其特征在于，基于所述光斑特征和所述初始位姿以及所述惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号的步骤包括：基于参考位姿和所述惯性测量数据，获得当前帧图像对应的预测位姿，所述预测位姿包括预测旋转信息；进行一次或多次迭代的跟踪匹配处理；
其中，每次跟踪匹配处理包括：随机选取两个光斑特征，作为随机光斑特征；估计所述随机光斑特征对应的最佳光斑序号；基于所述最佳光斑序号和所述预测旋转信息，获得可移动设备的初始位置信息；基于所述预测旋转信息和初始位置信息，获得所有定位灯在图像中的投影；将所述投影与所述光斑特征进行匹配，判断匹配数量和重投影误差是否满足匹配要求；如果是，则判断匹配成功，并输出当前的光斑序号；否则，重复进行跟踪匹配处理。
7.如权利要求6所述的位姿追踪方法，其特征在于，估计所述随机光斑特征对应的最佳光斑序号的步骤包括：根据所述预测位姿，获得可移动设备上的定位灯在图像上的投影，作为投影光斑；在投影光斑附近进行搜索匹配，获得与所述随机光斑匹配的投影光斑的序号，作为所述最佳光斑序号。
8.如权利要求6所述的位姿追踪方法，其特征在于，所述基于所述最佳光斑序号和预测旋转信息，获得可移动设备的初始位置信息的方法包括PnP算法。
9.如权利要求1所述的位姿追踪方法，其特征在于，通过紧耦合的方式对所述光斑序号和所述惯性测量数据进行融合的步骤包括：对所述惯性测量数据进行预积分，获得预积分数据；通过紧耦合的方式对所述光斑序号和所述预积分数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
10.如权利要求9所述的位姿追踪方法，其特征在于，利用平方根卡尔曼滤波器，通过紧耦合的方式对所述光斑序号和所述惯性测量数据进行融合，获得可移动设备的当前位姿。
11.如权利要求10所述的位姿追踪方法，其特征在于，所述平方根滤波器包括状态方程和观测方程；
其中，k-1时刻至k时刻的状态方程如下：
其中，状态估计量包括：角度误差ψk、位置误差Δpk、速度误差Δvk、陀螺零偏bgk、加速度计零偏bak；惯性测量数据的预积分增量包括：旋转增量位置增量/>速度增量
惯性测量数据的预积分增量对零偏的雅可比矩阵包括：旋转增量对陀螺零偏的雅可比矩阵JRg、位置增量对陀螺零偏的雅可比矩阵JPg、位置增量对加速度计零偏的雅可比矩阵JPa、速度增量对陀螺零偏的雅可比矩阵JVg、速度增量对加速度计零偏的雅可比矩阵JVa；状态传递噪声nk-1；
观测方程如下：
其中：z(1)，z(2)，...，z(n)为n个光斑特征的重投影误差；为第j个光斑特征的重投影误差的雅可比矩阵，νk为观测噪声。
12.如权利要求1所述的位姿追踪方法，其特征在于，所述基于所述图像，提取定位灯的光斑特征的步骤包括：基于所述图像，检测可移动设备框；从所述可移动设备框中提取所述定位灯的光斑特征。
13.如权利要求1所述的位姿追踪方法，其特征在于，所述位姿追踪方法还包括：在基于所述图像，提取定位灯的光斑特征之后，且在判断位姿是否已初始化之前，对所述光斑特征进行异常特征剔除处理，用于剔除异常的环境特征。
14.如权利要求1所述的位姿追踪方法，其特征在于，获取惯性测量数据包括：获取前一帧图像至当前帧图像之间的惯性测量数据，作为第一惯性测量数据；
所述位姿追踪方法还包括：在获得可移动设备在当前帧图像时刻的当前位姿后，获取当前帧图像至显示渲染时刻之间的惯性测量数据，作为第二惯性测量数据；基于所述第二惯性测量数据和所述当前位姿，获得显示渲染时刻的可移动设备位姿。
15.如权利要求14所述的位姿追踪方法，其特征在于，所述位姿追踪方法还包括：在获得显示渲染时刻的可移动设备位姿之后，对所述可移动设备位姿的数据进行平滑滤波处理。
16.如权利要求1至15任一项所述的位姿追踪方法，其特征在于，所述可移动设备为手柄。
17.一种位姿追踪***，其特征在于，包括：
图像处理模块，用于获取可移动设备的当前帧图像，所述可移动设备上设置有多个定位灯；图像处理模块还用于基于所述当前帧图像，提取定位灯的光斑特征；
惯性测量模块，用于获取惯性测量数据；
初始化判断模块，用于判断位姿是否已初始化；
跟踪匹配模块，用于在判断位姿已初始化时，获取前一帧图像对应的位姿作为参考位姿，并基于所述光斑特征和所述参考位姿以及所述惯性测量数据，进行跟踪匹配，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；
初始化搜索模块，用于在判断位姿未初始化时，基于所述光斑特征进行初始化搜索，获得光斑对应的定位灯在可移动设备上的序号，作为光斑序号；
视觉惯性融合模块，用于通过紧耦合的方式对所述光斑序号和所述惯性测量数据进行融合，获得可移动设备在当前帧图像时刻的当前位姿。
18.一种设备，其特征在于，包括至少一个存储器和至少一个处理器，所述存储器存储有一条或多条计算机指令，其中，所述一条或多条计算机指令被所述处理器执行以实现如权利要求1-16任一项所述的位姿追踪方法。
19.一种存储介质，其特征在于，所述存储介质存储有一条或多条计算机指令，所述一条或多条计算机指令用于实现如权利要求1-16任一项所述的位姿追踪方法。
CN202211623193.7A 2022-12-16 2022-12-16 位姿追踪方法以及***、设备及存储介质 Pending CN118212258A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202211623193.7A CN118212258A (zh) 2022-12-16 2022-12-16 位姿追踪方法以及***、设备及存储介质
PCT/CN2023/139055 WO2024125625A1 (en) 2022-12-16 2023-12-15 Pose tracking method and system, and apparatus and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211623193.7A CN118212258A (zh) 2022-12-16 2022-12-16 位姿追踪方法以及***、设备及存储介质

Publications (1)

Publication Number Publication Date
CN118212258A true CN118212258A (zh) 2024-06-18

Family

ID=91447560

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211623193.7A Pending CN118212258A (zh) 2022-12-16 2022-12-16 位姿追踪方法以及***、设备及存储介质

Country Status (2)

Country Link
CN (1) CN118212258A (zh)
WO (1) WO2024125625A1 (zh)

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10532277B2 (en) * 2015-06-11 2020-01-14 Facebook Technologies, Llc Hand-held controllers with light-emitting diodes synchronized to an external camera
US10719125B2 (en) * 2017-05-09 2020-07-21 Microsoft Technology Licensing, Llc Object and environment tracking via shared sensor
CN112527102B (zh) * 2020-11-16 2022-11-08 青岛小鸟看看科技有限公司 头戴式一体机***及其6DoF追踪方法和装置
CN114167979B (zh) * 2021-11-18 2024-04-26 玩出梦想(上海)科技有限公司 一种增强现实一体机手柄跟踪算法
CN115311353B (zh) * 2022-08-29 2023-10-10 玩出梦想(上海)科技有限公司 一种多传感器多手柄控制器图优化紧耦合追踪方法及***

Also Published As

Publication number Publication date
WO2024125625A1 (en) 2024-06-20

Similar Documents

Publication Publication Date Title
CN110979346B (zh) 确定车辆所处车道的方法、装置及设备
CN105654507B (zh) 一种基于图像动态特征跟踪的车辆外廓尺寸测量方法
JP2020518785A (ja) 車道の決定方法、装置及び記憶媒体
CN111094895B (zh) 用于在预构建的视觉地图中进行鲁棒自重新定位的***和方法
CN111623765B (zh) 基于多模态数据的室内定位方法及***
CN111210477A (zh) 一种运动目标的定位方法及***
US9412012B2 (en) Z-axis determination in a 2D gesture system
CN111998860A (zh) 自动驾驶定位数据校验方法、装置、电子设备及存储介质
CN110853085B (zh) 基于语义slam的建图方法和装置及电子设备
CN110648363A (zh) 相机姿态确定方法、装置、存储介质及电子设备
JP6265499B2 (ja) 特徴量抽出装置及び場所推定装置
CN111507204A (zh) 倒计时信号灯的检测方法、装置、电子设备及存储介质
WO2019127306A1 (en) Template-based image acquisition using a robot
CN109754034A (zh) 一种基于二维码的终端设备定位方法及装置
CN112991459A (zh) 一种相机标定方法、装置、设备以及存储介质
CN114969221A (zh) 一种更新地图的方法及相关设备
CN114332232B (zh) 基于空间点线面特征混合建模的智能手机室内定位方法
CN111832579A (zh) 地图兴趣点数据处理方法、装置、电子设备以及可读介质
CN113610702B (zh) 一种建图方法、装置、电子设备及存储介质
KR102249381B1 (ko) 3차원 영상 정보를 이용한 모바일 디바이스의 공간 정보 생성 시스템 및 방법
CN118212258A (zh) 位姿追踪方法以及***、设备及存储介质
CN111194015A (zh) 基于建筑物的室外定位方法、装置及移动设备
CN105468881A (zh) 基于航拍图像的实景距离计算方法及装置
US9842402B1 (en) Detecting foreground regions in panoramic video frames
US9824455B1 (en) Detecting foreground regions in video frames

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication