一种跟踪系统初始化方法及终端设备与流程

本申请涉及相机跟踪技术，尤其涉及一种跟踪系统初始化方法及终端设备。

背景技术：

视觉与惯导融合的相机跟踪技术在移动增强现实、机器人视觉导航等领域有着重要的用途。针对单目相机跟踪系统的初始化尤为困难，这是因为从单目相机进行空间三维重建时存在着天然的尺度不确定性。而初始化尺度的误差会进一步带来后续跟踪的不稳定。因此视觉惯导相机跟踪系统的快速鲁棒初始化对于整个系统的平稳运行有着非常关键的作用。

目前已有的初始化方案可以简单分为两大类：基于卡尔曼滤波的直接传感器融合方案(简称为滤波方案)和基于纯视觉跟踪再进行惯导对齐的方案(简称为对齐方案)。在滤波方案下，测量偏差的错误初值会使得收敛变慢，而在对齐方案下，已有的方案难以同时求解这两个分量，带来尺度估计上的误差。

技术实现要素：

为解决上述技术问题，本申请实施例提供了一种跟踪系统初始化方法及终端设备、存储介质、计算机程序产品、计算机设备。

本申请实施例提供的跟踪系统初始化方法，包括：

采集视觉数据，基于所述视觉数据恢复图像采集装置的运动数据；

将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到初步的尺度信息和初步的重力信息；

基于所述视觉数据中的竖直边缘信息对所述初步的重力信息进行修正，得到精确的重力信息，并基于所述精确的重力信息确定精确的尺度信息。

本申请实施例中，所述采集视觉数据，基于所述视觉数据恢复图像采集装置的运动数据，包括：

采集多帧图像；

对所述多帧图像进行视觉重建处理，得到所述图像采集装置的多个位姿信息，其中，所述多帧图像中的每帧图像对应一个所述图像采集装置的位姿信息，所述图像采集装置的位姿信息包括所述图像采集装置的位置信息和姿态信息。

本申请实施例中，所述将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐之前，所述方法还包括：

测量所述图像采集装置的惯导数据，并对所述惯导数据进行积分处理，其中，所述惯导数据包括角速度和加速度。

本申请实施例中，所述将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到初步的尺度信息和初步的重力信息，包括：

基于所述图像采集装置的多个位姿信息和所述惯导数据的积分结果，建立多个运动约束方程；

基于所述多个运动约束方程，得到所述初步的尺度信息和初步的重力信息。

本申请实施例中，所述基于所述图像采集装置的多个位姿信息和所述惯导数据的积分结果，建立多个运动约束方程，包括：

基于所述图像采集装置的任意两个位姿信息和所述惯导数据的积分结果，建立多个运动约束方程；其中，所述图像采集装置的任意两个位姿信息包括：所述图像采集装置的任意两个相邻的位姿信息和任意两个非相邻的位姿信息。

本申请实施例中，所述基于所述视觉数据中的竖直边缘信息对所述初步的重力信息进行修正，得到精确的重力信息，包括：

从所述视觉数据中提取直线边缘信息；

根据所述初步的重力信息对所述直线边缘信息进行过滤，得到与所述重力信息的误差在预设阈值内的直线边缘信息，将与所述重力信息的误差在预设阈值内的直线边缘信息作为竖直边缘信息；

将所述竖直边缘信息和所述重力信息一致作为约束条件建立线性方程，基于所述线性方程得到所述精确的重力信息。

本申请实施例中，所述竖直边缘信息的数量为多个，多个所述竖直边缘信息对应多个所述线性方程；

所述基于所述线性方程得到所述精确的重力信息，包括：

根据多个所述竖直边缘信息中的每个所述竖直边缘信息的长度对所述多个所述线性方程进行加权后联立；

基于联立后的线性方程，得到所述精确的重力信息。

本申请实施例中，所述基于所述精确的重力信息获取精确的尺度信息，包括：

利用所述精确的重力信息作为约束，将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到所述精确的尺度信息。

本申请实施例提供的终端设备，包括：

图像采集装置，用于采集视觉数据；

惯导测量装置，用于测量所述图像采集装置的惯导数据；

视觉重建模块，用于基于所述视觉数据恢复图像采集装置的运动数据；

初步对齐模块，用于将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到初步的尺度信息和初步的重力信息；

优化模块，用于基于所述视觉数据中的竖直边缘信息对所述初步的重力信息进行修正，得到精确的重力信息，并基于所述精确的重力信息确定精确的尺度信息。

本申请实施例中，所述图像采集装置，用于采集多帧图像；

所述视觉重建模块，用于对所述多帧图像进行视觉重建处理，得到所述图像采集装置的多个位姿信息，其中，所述多帧图像中的每帧图像对应一个所述图像采集装置的位姿信息，所述图像采集装置的位姿信息包括所述图像采集装置的位置信息和姿态信息。

本申请实施例中，所述装置还包括：

预积分模块，用于对所述惯导数据进行积分处理，其中，所述惯导数据包括角速度和加速度。

本申请实施例中，所述初步对齐模块，用于基于所述图像采集装置的多个位姿信息和所述惯导数据的积分结果，建立多个运动约束方程；基于所述多个运动约束方程，得到所述初步的尺度信息和初步的重力信息。

本申请实施例中，所述初步对齐模块，用于基于所述图像采集装置的任意两个位姿信息和所述惯导数据的积分结果，建立多个运动约束方程；其中，所述图像采集装置的任意两个位姿信息包括：所述图像采集装置的任意两个相邻的位姿信息和任意两个非相邻的位姿信息。

本申请实施例中，所述优化模块，用于从所述视觉数据中提取直线边缘信息；根据所述初步的重力信息对所述直线边缘信息进行过滤，得到与所述重力信息的误差在预设阈值内的直线边缘信息，将与所述重力信息的误差在预设阈值内的直线边缘信息作为竖直边缘信息；将所述竖直边缘信息和所述重力信息一致作为约束条件建立线性方程，基于所述线性方程得到所述精确的重力信息。

本申请实施例中，所述竖直边缘信息的数量为多个，多个所述竖直边缘信息对应多个所述线性方程；

所述优化模块，用于根据多个所述竖直边缘信息中的每个所述竖直边缘信息的长度对所述多个所述线性方程进行加权后联立；基于联立后的线性方程，得到所述精确的重力信息。

本申请实施例中，所述优化模块，用于利用所述精确的重力信息作为约束，将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到所述精确的尺度信息。

本申请实施例提供的计算机程序产品，包括计算机可执行指令，该计算机可执行指令被执行后，能够实现上述的跟踪系统初始化方法。

本申请实施例提供的存储介质上存储有可执行指令，该可执行指令被处理器执行时实现上述的跟踪系统初始化方法。

本申请实施例提供的计算机设备，包括存储器和处理器，所述存储器上存储有计算机可执行指令，所述处理器运行所述存储器上的计算机可执行指令时可实现上述的跟踪系统初始化方法。

本申请实施例的技术方案中，采集视觉数据，基于所述视觉数据恢复图像采集装置的运动数据；将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到初步的尺度信息和初步的重力信息；基于所述视觉数据中的竖直边缘信息对所述初步的重力信息进行修正，得到精确的重力信息，并基于所述精确的重力信息确定精确的尺度信息。采用本申请实施例的技术方案，通过视觉数据中的竖直边缘信息更准确地对重力信息进行估计，使得初始化精度显著提高，尺度估计更为准确，从而可以更快完成更高质量的跟踪系统初始化。

附图说明

图1为本申请实施例提供的跟踪系统初始化方法的流程示意图一；

图2为本申请实施例提供的跟踪系统初始化方法的流程示意图二；

图3为本申请实施例提供的终端设备的结构组成示意图；

图4为本申请实施例的计算机设备的结构组成示意图。

具体实施方式

现在将参照附图来详细描述本申请的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本申请的范围。

同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。

以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本申请及其应用或使用的任何限制。

对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

本申请实施例可以应用于移动终端，例如：手机、平板电脑、地面机器人、无人机、车载终端等等。这类移动终端具有跟踪系统，通过该跟踪系统可以实现移动终端的定位、导航和跟踪。跟踪系统在正常工作之前，需要进行初始化，相关的初始化方案可以简单分为两大类：基于卡尔曼滤波的直接传感器融合方案和基于纯视觉跟踪再进行惯导对齐的方案。基于卡尔曼滤波的直接传感器融合方案无法真正解决初始状态估计的问题，因此需要非常好的初始值(简称为初值)，否则会直接导致初始化失败。为了控制这些参数的初值，基于卡尔曼滤波的方案需要在良好可控的实验室环境下进行初始化，这使得这类方法很难直接应用于移动增强现实等真实场景。此外，基于滤波的方案的精度和收敛速度已被广泛地证明要逊色于基于优化的方案，这使得基于滤波的初始化需要更多的时间完成良好的收敛。相比之下，基于纯视觉跟踪再进行惯导对齐的方案对初值要更为鲁棒，这类方法通过纯视觉的跟踪来重建恢复相机的相对运动，然而，恢复得到的相对运动缺失了绝对尺度信息(简称为尺度信息)以及在真实空间中的朝向信息(例如重力方向信息)，因此，需要再将其与惯导传感器得到的数据(本申请实施例称为惯导数据)进行对齐以恢复这些缺失的信息。为了进行对齐，就需要积累一定时间窗口内的数据，因此对齐方案下，初始化的精度和速度就存在着矛盾。一方面，诸多应用需要能够在尽可能短的时间内完成初始化，这就意味着系统只能得到有限帧的图像和有限个数的惯导数据的采样。在帧数少的情况下，惯导传感器的相对运动也较小，视觉和惯导的对齐会存在很大的误差。为了降低误差，需要积累更多的图像和惯导数据，可这导致了初始化缓慢的问题，同时更多的数据带来了更大的计算量，加重了系统的负担。为此，本申请实施例提出了一种跟踪系统初始化方法，通过对少量的图像和惯导数据进行对其就可以恢复出高精度的缺失信息，从而实现快速且高精度的初始化。

图1为本申请实施例提供的跟踪系统初始化方法的流程示意图一，如图1所示，所述跟踪系统初始化方法包括以下步骤：

步骤101：采集视觉数据，基于所述视觉数据恢复图像采集装置的运动数据。

本申请实施例的技术方案应用于终端设备中，该终端设备可以但不限于是手机、平板电脑、地面机器人、无人机、车载终端等等。这类终端设备具有跟踪系统，例如视觉惯导-即时定位与地图构建(visualinertial-simultaneouslocalizationandmapping，vi-slam)系统。vi-slam系统需要一个初始化过程，来初始化惯导测量装置的状态、图像采集装置的位置和姿态(统称为位姿)。具体来说，初始化的有图像采集装置的三维坐标和三维朝向、惯导测量装置的角速度偏差和加速度偏差、以及图像采集装置或惯导测量装置的初始速度。

具体实现时，上述惯导测量装置可以是惯性测量单元(inertialmeasurementunit，imu)，上述图像采集装置可以是相机。

本申请实施例中，通过纯视觉的方案来重建恢复图像采集装置的运动数据，具体地，采集多帧图像；对所述多帧图像进行视觉重建处理，得到所述图像采集装置的多个位姿信息，其中，所述多帧图像中的每帧图像对应一个所述图像采集装置的位姿信息，所述图像采集装置的位姿信息包括所述图像采集装置的位置信息和姿态信息。

举个例子：图像采集装置采集了4帧图像，分别为图像1、图像2、图像3、图像4，这4帧图像是图像采集装置以不同的位姿(不同的位置和/不同的姿态)，对同一场景进行拍摄得到。通过对这4帧图像进行视觉重建处理，可以得到图像采集装置采集各帧图像对应的位姿信息。例如以位姿1采集图像1，以位姿2采集图像2，位姿3采集图像3，位姿4采集图像4。需要说明的是，图像采集装置的位姿信息包括了图像采集状态的位置信息和姿态信息，其中，位置信息表征了图像采集装置作为一个整体在三维空间中的位置，例如通过三个位置坐标(简称为三维坐标)来表示位置信息；姿态信息表征了图像采集装置的整体姿态，例如通过三个姿态角(简称为三维朝向)来表示姿态信息。

步骤102：将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到初步的尺度信息和初步的重力信息。

然而，恢复得到的运动数据缺失了绝对尺度信息(简称为尺度信息)以及在真实空间中的朝向信息(例如重力方向信息)，因此，需要再将其与惯导测量装置测量得到的惯导数据进行对齐以恢复这些缺失的信息。这里，可以通过但不局限于imu来测量得到惯导数据。需要说明的是，本申请实施例可以通过少量的图像和惯导数据进行对齐，来恢复这些缺失的信息。由于图像和惯导数据的数量较少，因而将此次初步对齐得到的尺度信息称为初步的尺度信息(即精度较低的尺度信息)，同样，将此次初步对齐得到的重力信息称为初步的重力信息(即精度较低的重力信息)。

本申请实施例中，对齐的原理是：将恢复得到的所述图像采集装置的运动数据和测量得到的惯导数据均作为已知量，将尺度信息和重力信息作为未知量，构建方程，对该方程进行求解得到尺度信息和重力信息。

步骤103：基于所述视觉数据中的竖直边缘信息对所述初步的重力信息进行修正，得到精确的重力信息，并基于所述精确的重力信息确定精确的尺度信息。

上述步骤得到的初步的尺度信息和初步的重力信息的精度较低，且通过对初始化中的误差来源进行分析和建模，可以发现惯导测量装置内在的测量偏差是带来精度较低的主要原因，特别是加速度的测量结果是图像采集装置的实际加速度与重力加速度混合在一起。

本申请实施例基于所述视觉数据中的竖直边缘信息对所述初步的重力信息进行修正，得到精确的重力信息。这里，通过利用图像的竖直边缘信息可以提供额外的重力方向信息，从而解决惯导数据对齐时重力与加速度偏差无法区分的问题。

图2为本申请实施例提供的跟踪系统初始化方法的流程示意图二，如图2所示，所述跟踪系统初始化方法包括以下步骤：

步骤201：采集视觉数据，基于所述视觉数据恢复图像采集装置的运动数据。

本申请实施例的技术方案应用于终端设备中，该终端设备可以但不限于是手机、平板电脑、地面机器人、无人机、车载终端等等。这类终端设备具有跟踪系统，例如vi-slam系统。vi-slam系统需要一个初始化过程，来初始化惯导测量装置的状态、图像采集装置的位置和姿态。具体来说，初始化的有图像采集装置的三维坐标和三维朝向、惯导测量装置的角速度偏差和加速度偏差、以及图像采集装置或惯导测量装置的初始速度。

具体实现时，上述惯导测量装置可以是imu，上述图像采集装置可以是相机。

本申请实施例中，通过纯视觉的方案来重建恢复图像采集装置的运动数据，具体地，采集多帧图像；对所述多帧图像进行视觉重建处理，得到所述图像采集装置的多个位姿信息，其中，所述多帧图像中的每帧图像对应一个所述图像采集装置的位姿信息，所述图像采集装置的位姿信息包括所述图像采集装置的位置信息和姿态信息。

举个例子：图像采集装置采集了4帧图像，分别为图像1、图像2、图像3、图像4，这4帧图像是图像采集装置以不同的位姿(不同的位置和/不同的姿态)，对同一场景进行拍摄得到。通过对这4帧图像进行视觉重建处理，可以得到图像采集装置采集各帧图像对应的位姿信息。例如以位姿1采集图像1，以位姿2采集图像2，位姿3采集图像3，位姿4采集图像4。需要说明的是，图像采集装置的位姿信息包括了图像采集状态的位置信息和姿态信息，其中，位置信息表征了图像采集装置作为一个整体在三维空间中的位置，例如通过三个位置坐标(简称为三维坐标)来表示位置信息；姿态信息表征了图像采集装置的整体姿态，例如通过三个姿态角(简称为三维朝向)来表示姿态信息。

步骤202：测量所述图像采集装置的惯导数据，并对所述惯导数据进行积分处理，其中，所述惯导数据包括角速度和加速度。

这里，可以通过但不局限于imu来测量得到惯导数据。所述惯导数据包括角速度和加速度，其中，角速度包含了旋转轴的速度和绕轴旋转的速度。

然而，恢复得到的运动数据缺失了绝对尺度信息(简称为尺度信息)以及在真实空间中的朝向信息(例如重力方向信息)，因此，需要再将其与惯导测量装置测量得到的惯导数据进行对齐以恢复这些缺失的信息。需要说明的是，本申请实施例可以通过少量的图像和惯导数据进行对齐，来恢复这些缺失的信息。由于图像和惯导数据的数量较少，因而将此次初步对齐得到的尺度信息称为初步的尺度信息(即精度较低的尺度信息)，同样，将此次初步对齐得到的重力信息称为初步的重力信息(即精度较低的重力信息)。

本申请实施例中，在进行视觉惯导对齐之前，预先对所述惯导数据进行积分，这是由于惯导测量装置测量得到的惯导数据包括角速度和加速度，对角速度积分后才能与步骤201中视觉重建得到的姿态信息进行对齐，对加速度进行积分后才能与步骤201中视觉重建得到的位置信息进行对齐。

步骤203：基于所述图像采集装置的多个位姿信息和所述惯导数据的积分结果，建立多个运动约束方程；基于所述多个运动约束方程，得到所述初步的尺度信息和初步的重力信息。

本申请实施例中，对齐的原理是：将恢复得到的所述图像采集装置的运动数据和惯导数据的积分结果均作为已知量，将尺度信息和重力信息作为未知量，构建方程，对该方程进行求解得到尺度信息和重力信息。

具体地，基于所述图像采集装置的任意两个位姿信息和所述惯导数据的积分结果，建立多个运动约束方程；其中，所述图像采集装置的任意两个位姿信息包括：所述图像采集装置的任意两个相邻的位姿信息和任意两个非相邻的位姿信息。

这里，建立的运动约束方程是针对所有帧间运动关系的，举个例子，假设按照时间先后顺序，图像采集装置在t1时刻以位姿1采集图像1，在t2时刻以位姿2采集图像2，在t3时刻以位姿3采集图像3，在t4时刻以位姿4采集图像4。这4帧图像是图像采集装置以不同的位姿(不同的位置和/不同的姿态)，对同一场景进行拍摄得到。通过对这4帧图像进行视觉重建处理，可以得到图像采集装置采集各帧图像对应的位姿信息。另一方面，图像采集装置从t1时刻到t2时刻，再到t3时刻，而后到t4时刻是发生运动的，惯导测量装置会同时测量实时的惯导数据(角速度和加速度)，可以建立如下6个运动约束方程：

位姿1、位姿2、惯导数据从t1时刻到t2时刻的积分、尺度信息、重力信息、图像采集装置的速度之间的运动约束方程1；

位姿1、位姿3、惯导数据从t1时刻到t3时刻的积分、尺度信息、重力信息、图像采集装置的速度之间的运动约束方程2；

位姿1、位姿4、惯导数据从t1时刻到t4时刻的积分、尺度信息、重力信息、图像采集装置的速度之间的运动约束方程3；

位姿2、位姿3、惯导数据从t2时刻到t3时刻的积分、尺度信息、重力信息、图像采集装置的速度之间的运动约束方程4；

位姿2、位姿4、惯导数据从t2时刻到t4时刻的积分、尺度信息、重力信息、图像采集装置的速度之间的运动约束方程5；

位姿3、位姿4、惯导数据从t3时刻到t4时刻的积分、尺度信息、重力信息、图像采集装置的速度之间的运动约束方程6。

上述6个运动约束方程中，既包括了两个相邻的位姿信息对应的运动约束方程，也包括两个非相邻的位姿信息对应的运动约束方程，更全面的运动约束方程允许在帧数较少时依旧能够最大利用有效的运动，得到精度较高的重力信息。

在一个示例中，上述运动约束方程可以通过以下公式表示：

其中，位置j是指位姿j中包含的位置信息，位置i是指位姿i中包含的位置信息，位姿j是指j时刻图像采集装置的位姿，位姿i是指i时刻图像采集装置的位姿，ij间惯导信息积分为i时刻到j时刻之间惯导测量装置测量得到的惯导数据的积分，t为i时刻到j时刻之间的时长，速度i为图像采集装置在i时刻的速度。这里，位姿信息是对视觉数据进行视觉重建处理得到，因而为已知量；ij间惯导信息积分是对测量得到的惯导数据进行积分得到，因而也为已知量；尺度参数是未知量，上述公式将未知的尺度参数与真实世界的运动数据联系起来；重力参数同样也是未知量，这是因为视觉重建处理无法直接恢复重力的大小和方向，此外，速度i作为系统隐含的状态，同样是未知量。

需要说明的是，上述运动约束方程为线性的运动约束方程，在运动约束方程中，位姿和惯导数据的积分为已知量，尺度信息、重力信息以及图像采集装置的速度为未知量。本申请实施例对全体位姿两两建立运动约束方程，将全部的运动约束方程联立求解可得到初步的尺度信息、初步的重力信息等。这里，重力信息尤其指重力方向信息。

本申请实施例的技术方案，在进行视觉惯导对齐时，不仅考虑相邻帧之间的运动约束，还加入了远距离帧之间的运动约束，这使得求解的方程中包含了充分的运动信息，解决了相邻帧间运动较小信息不足的问题。

步骤204：从所述视觉数据中提取直线边缘信息；根据所述初步的重力信息对所述直线边缘信息进行过滤，得到与所述重力信息的误差在预设阈值内的直线边缘信息，将与所述重力信息的误差在预设阈值内的直线边缘信息作为竖直边缘信息；将所述竖直边缘信息和所述重力信息一致作为约束条件建立线性方程，基于所述线性方程得到所述精确的重力信息。

这里，直线边缘信息与重力信息的误差是指直线边缘的方向与重力的方向的角度误差。

上述步骤得到的初步的尺度信息和初步的重力信息的精度较低，且通过对初始化中的误差来源进行分析和建模，可以发现惯导测量装置内在的测量偏差是导致精度较低的主要原因，特别是加速度的测量结果是图像采集装置的实际加速度与重力加速度混合在一起。

本申请实施例基于所述视觉数据中的竖直边缘信息对所述初步的重力信息进行修正，得到精确的重力信息。这里，通过利用图像的竖直边缘信息可以提供额外的重力方向信息，从而解决惯导数据对齐时重力与加速度偏差无法区分的问题。

本申请实施例中，以所述视觉数据中的一帧图像为例，从图像中提取直线边缘信息，然后结合初步的重力信息(带有误差的重力信息)从提取出的直线边缘信息中过滤出能够真正反映场景重力方向的竖直边缘信息，最后基于竖直边缘信息恢复更为准确的重力方向(也即精确的重力信息)。

在一个场景中，图像中的直线边缘信息可能有竖直方向的，也可能有水平方向的，或者有其他倾斜角度方向的，在这些直线边缘中保留与初步的重力信息的误差在预设阈值内的直线边缘信息作为竖直边缘信息。这里，由于所述初步的重力信息比较接近真实的重力方向信息，因此，可以以此为基准来确定一个范围内的直线边缘信息作为竖直边缘信息，而真实的重力方向信息可以基于这些竖直边缘信息来确定。举个例子：初步的重力信息为方向1，可以将与方向1的夹角小于等于某个特定的阈值的直线边缘作为竖直边缘。进一步，所述竖直边缘信息的数量为多个，多个所述竖直边缘信息对应多个线性方程；根据多个所述竖直边缘信息中的每个所述竖直边缘信息的长度对所述多个所述线性方程进行加权后联立；基于联立后的线性方程，得到所述精确的重力信息。需要说明的是，上述线性方程中精确的重力方向是唯一的未知量。

步骤205：利用所述精确的重力信息作为约束，将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到所述精确的尺度信息。

本申请实施例中，基于上述步骤204得到的精确的重力方向，将步骤201中视觉重建的结果变换到该重力方向的坐标系下，使得重力向下，并利用步骤203中初步对齐得到的初步的尺度信息对变换结果进行缩放。最后，将图像采集装置的位姿信息、惯导状态、点云坐标等纳入统一的集束优化中进行优化，得到最终的初始化结果。

本申请实施例的技术方案，可以加快视觉惯导跟踪系统的初始化速度。以往，用户在启动跟踪系统后，需要进行一段时间的移动以充分初始化系统。应用本初始化算法后，跟踪系统基本能在出现运动的第一时间完成初始化。此外，通过初始化的结果，可以对场景的尺度进行准确的估计。在系统完成初始化后，跟踪系统可以更准确恢复场景中的真实尺度，从而允许对场景中的距离进行准确的测量。

图3为本申请实施例提供的终端设备的结构组成示意图，如图3所示，所述终端设备包括：

图像采集装置301，用于采集视觉数据；

惯导测量装置302，用于测量所述图像采集装置的惯导数据；

视觉重建模块303，用于基于所述视觉数据恢复图像采集装置的运动数据；

初步对齐模块304，用于将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到初步的尺度信息和初步的重力信息；

优化模块305，用于基于所述视觉数据中的竖直边缘信息对所述初步的重力信息进行修正，得到精确的重力信息，并基于所述精确的重力信息确定精确的尺度信息。

在一实施方式中，所述图像采集装置301，用于采集多帧图像；

所述视觉重建模块303，用于对所述多帧图像进行视觉重建处理，得到所述图像采集装置的多个位姿信息，其中，所述多帧图像中的每帧图像对应一个所述图像采集装置的位姿信息，所述图像采集装置的位姿信息包括所述图像采集装置的位置信息和姿态信息。

在一实施方式中，所述装置还包括：

预积分模块306，用于对所述惯导数据进行积分处理，其中，所述惯导数据包括角速度和加速度。

在一实施方式中，所述初步对齐模块304，用于基于所述图像采集装置的多个位姿信息和所述惯导数据的积分结果，建立多个运动约束方程；基于所述多个运动约束方程，得到所述初步的尺度信息和初步的重力信息。

在一实施方式中，所述初步对齐模块304，用于基于所述图像采集装置的任意两个位姿信息和所述惯导数据的积分结果，建立多个运动约束方程；其中，所述图像采集装置的任意两个位姿信息包括：所述图像采集装置的任意两个相邻的位姿信息和任意两个非相邻的位姿信息。

在一实施方式中，所述优化模块305，用于从所述视觉数据中提取直线边缘信息；根据所述初步的重力信息对所述直线边缘信息进行过滤，得到与所述重力信息的误差在预设阈值内的直线边缘信息，将与所述重力信息的误差在预设阈值内的直线边缘信息作为竖直边缘信息；将所述竖直边缘信息和所述重力信息一致作为约束条件建立线性方程，基于所述线性方程得到所述精确的重力信息。

在一实施方式中，所述竖直边缘信息的数量为多个，多个所述竖直边缘信息对应多个所述线性方程；

所述优化模块305，用于根据多个所述竖直边缘信息中的每个所述竖直边缘信息的长度对所述多个所述线性方程进行加权后联立；基于联立后的线性方程，得到所述精确的重力信息。

在一实施方式中，所述优化模块305，用于利用所述精确的重力信息作为约束，将恢复得到的所述图像采集装置的运动数据与测量得到的惯导数据进行对齐，得到所述精确的尺度信息。

本领域技术人员应当理解，图3所示的终端设备中的各模块的实现功能可参照前述跟踪系统初始化方法的相关描述而理解。图3所示的终端设备中的各模块的功能可通过运行于处理器上的程序而实现，也可通过具体的逻辑电路而实现。

本发明实施例上述基于终端设备中的各模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，readonlymemory)、磁碟或者光盘等各种可以存储程序代码的介质。这样，本发明实施例不限制于任何特定的硬件和软件结合。

相应地，本发明实施例还提供一种计算机程序产品，其中存储有计算机可执行指令，该计算机可执行指令被执行时能够实现本发明实施例的上述跟踪系统初始化方法。

图4为本发明实施例的计算机设备的结构组成示意图，如图4所示，计算机设备100可以包括一个或多个(图中仅示出一个)处理器1002(处理器1002可以包括但不限于微处理器(mcu，microcontrollerunit)或可编程逻辑器件(fpga，fieldprogrammablegatearray)等的处理装置)、用于存储数据的存储器1004、以及用于通信功能的传输装置1006。本领域普通技术人员可以理解，图4所示的结构仅为示意，其并不对上述电子装置的结构造成限定。例如，计算机设备100还可包括比图4中所示更多或者更少的组件，或者具有与图4所示不同的配置。

存储器1004可用于存储应用软件的软件程序以及模块，如本发明实施例中的方法对应的程序指令/模块，处理器1002通过运行存储在存储器1004内的软件程序以及模块，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器1004可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器1004可进一步包括相对于处理器1002远程设置的存储器，这些远程存储器可以通过网络连接至计算机设备100。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

传输装置1006用于经由一个网络接收或者发送数据。上述的网络具体实例可包括计算机设备100的通信供应商提供的无线网络。在一个实例中，传输装置1006包括一个网络适配器(nic，networkinterfacecontroller)，其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输装置1006可以为射频(rf，radiofrequency)模块，其用于通过无线方式与互联网进行通讯。

本发明实施例所记载的技术方案之间，在不冲突的情况下，可以任意组合。

在本发明所提供的几个实施例中，应该理解到，所揭露的方法和智能设备，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。

上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。

另外，在本发明各实施例中的各功能单元可以全部集成在一个第二处理单元中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。