定位初始化方法和相关装置、设备、存储介质

技术领域

本申请涉及定位技术领域，特别是涉及一种定位初始化方法和相关装置、设备、存储介质。

背景技术

目前，视觉定位方法在计算机视觉、机器人、无人机、三维重建一级增强现实等领域中起着重要的作用。一般情况下，对视觉定位所需状态量进行初始化所需的初始图像至少都是经过很长时间拍摄得到的，也即是需要使用的初始图像数量至少都是上百张，这就导致在初始化的耗时较长且初始化所需计算量过大。

发明内容

本申请至少提供一种定位初始化方法和相关装置、设备、存储介质。

本申请第一方面提供了一种定位初始化方法，包括：获取第一预设数量帧初始图像，其中，第一预设数量大于或等于4，且小于预设帧数阈值，预设帧数阈值由执行设备的计算能力确定；利用第一预设数量帧初始图像进行视觉初始化，得到视觉初始化结果，其中，视觉初始化结果包括初始图像的位姿；结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化。

因此，通过根据执行设备的计算能力确定预设帧数阈值，以使用至少四帧且小于预设帧数阈值的初始图像即可完成初始化，相较于一般需要使用大量的初始图像完成初始化的方案而言，前者初始化的速度更快以及降低了初始化的计算量。

其中，结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化，包括：将各初始图像之间的惯性传感器信息与视觉初始化结果进行对齐处理，得到经初始化的定位状态量信息，其中，对齐处理中所使用的惯性传感器的偏置量为预设值。

因此，通过将惯性传感器的偏置量定为预设值，减小因为惯性传感器的偏置量与重力和尺度过度耦合造成对尺度和/或重力的进行错误的估值的情况出现，从而降低错误的偏置量对初始化稳定性造成的影响。

其中，利用预设数量的初始图像进行视觉初始化操作得到视觉初始化结果，包括：以其中一帧初始图像作为参考图像，并确定至少一帧第一初始图像与参考图像之间的第一相对位置关系，其中，第一初始图像为除参考图像以外的初始图像；根据第一相对位置关系，确定至少一帧第一初始图像与参考图像中的共同观测二维点对应的第一三维点的位置信息；利用第一三维点的位置信息，确定第二初始图像与参考图像之间的第二相对位置关系，其中，第二初始图像为除第一初始图像和参考图像之外的初始图像，第一初始图像、第二初始图像和参考图像的数量总和为第一预设数量。

因此，通过先确定部分初始图像的位置，再确定其余初始图像的位置加快了视觉初始化的速度。

其中，相对位置关系包括距离和/或相对角度；确定至少一帧第一初始图像与参考图像之间的第一相对位置关系，包括：将第一初始图像与参考图像之间的距离确定为预设距离值；和/或，基于第一初始图像与参考图像之间的惯性传感器预积分，得到第一初始图像与参考图像之间的相对角度，或以对极约束的方式获取第一初始图像与参考图像之间的相对角度。

因此，通过预设距离值以及通过惯性传感器/对极约束的方式可以获取参考图像和第一初始图像之间的距离和位置即可获得参考图像和第一初始图像之间的相对位置关系。

其中，在利用第一三维点的位置信息，确定第二初始图像与参考图像之间的第二相对位置关系之后，方法还包括：确定存在于第二预设数量帧初始图像的若干二维点，其中，若干二维点不对应于第一三维点；获取确定的二维点对应的第二三维点的位置信息；优化初始图像的位姿、第一三维点的位置信息和第二三维点的位置信息中的至少一种。

因此，通过确定至少存在与第二预设数量帧初始图像上且未与第一三维点对应的二维点，并获取其对应的三维点信息，再联合第一三维点和第二三维点的位置信息以及初始图像的位姿联合优化，使得视觉初始化的结果更准确。

其中，结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化是在确定视觉初始化处理成功之后执行的；方法还包括以下至少一个步骤，以确定视觉初始化处理成功：确定第一三维点的数量大于第一预设阈值；确定深度为正的三维点的数量大于第二预设阈值，其中，三维点包括第一三维点和第二三维点；确定三维点在初始图像上的平均重投影误差小于或等于第三预设阈值。

因此，通过判断视觉初始化成功之后再结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化，减少视觉初始化失败的结果与惯性传感器之间错误结合造成计算资源的浪费。

其中，方法还包括以下至少一个步骤，以确定定位状态量的初始化成功：确定经初始化的至少部分定位状态量处于第一预设范围；基于经初始化的定位状态量确定传感数据，且确定传感数据处于第二预设范围；其中，传感数据包括重力大小、惯性传感器的偏置量中的一者或多者。

因此，通过判断初始化得到的定位状态量是否在合理的范围，判断初始化是否成功，减少将错误的初始化结果用于定位的情况出现。

其中，在定位状态量进行初始化成功之后，方法还包括：确定第三初始图像在世界坐标系的预设位置，其中，第三初始图像为满足预设要求的初始图像；基于第三初始图像所确定的位置以及相邻初始图像之间的相对位置关系，调整除第三初始图像以外的其余初始图像在世界坐标系上的位置。

因此，通过固定一初始图像在世界坐标下的位置，能够确定其余初始图像在世界坐标系下的位置。

其中，预设要求为拍摄时间最早；和/或，方法还包括以下至少一个步骤，以设置状态量的权重：将第三初始图像的位置的权重设置为第一权重；将第三始图像的航向角的权重设置为第二权重；将惯性传感器中的陀螺仪的偏置量的权重设置为第三权重；将第一预设数量帧初始图像的重力方向、速度以及惯性传感器中的加速度计的偏置量对应的权重分别设置为第四权重、第五权重以及第六权重；其中，状态量的权重越大，对应的不确定度越低。

因此，因为拍摄得到的首帧初始图像的位置以及其航向角在世界坐标系上的位置不可知，将首帧初始图像的位置设置为世界坐标系的预设位置以及其航向角并给予较大的权重，使得后续通过该位置调整得到其余初始图像的位置更可靠。

其中，第一权重、第二权重以及第三权重均大于第四权重、第五权重以及第六权重。

因此，通过确定第一权重至第六权重之间的大小关系，使得后续在对目标进行定位或者对初始化后的状态量进行更新的过程中，能够更多的依靠不确定度低的状态量去更新不确定度高的状态量。

其中，方法还包括以下至少一个步骤：若视觉初始化失败，则确定定位初始化失败；若对定位状态量进行初始化失败，则确定定位初始化失败；在执行利用第一预设数量帧初始图像进行视觉初始化之前，判断是否存在相邻初始图像之间的第一运动幅度大于第一预设幅度；若存在，则执行利用第一预设数量帧初始图像进行视觉初始化，否则确定定位初始化失败。

因此，若运动幅度过小，则对应的三维点数量可能过少，得到的视觉初始化结果可能不太准确，直接认定定位初始化失败以减小计算量。

其中，方法还包括：在确定定位初始化失败的情况下，删除第一预设数量帧中拍摄时间最早的初始图像以及初始化过程中的至少部分处理结果。

因此，通过初始化失败之后，删除拍摄时间最早的初始图像使得下次可以使用新的初始图像进行初始化，提高初始化的成功率。

其中，获取第一预设数量帧初始图像，包括以下至少一个步骤：将每间隔预设时间拍摄得到的图像作为初始图像；获取拍摄得到的待定图像，并判断待定图像与第四初始图像之间的第二运动幅度是否大于第二预设幅度，在第二运动幅度大于第二预设幅度的情况下，确定待定图像为初始图像；其中，第四初始图像为与待定图像之间的时间差最小的初始图像。

因此，固定获取初始图像的间隔时间或通过运动幅度获取初始图像，而是将拍摄得到的所有图像帧作为初始图像，能够降低初始化过程中的计算量。

其中，判断待定图像与第四初始图像之间的第二运动幅度是否大于第二预设幅度，包括：判断待定图像与第四初始图像之间的视差是否大于预设视差，在视差大于预设视差的情况下，确定第二运动幅度大于第二预设幅度；或，判断待定图像与第四初始图像之间的惯性传感器的预积分数值是否大于预设积分数值，在预积分数值大于预设积分数值的情况下，确定第二运动幅度大于第二预设幅度。

因此，通过视差和/或预积分数值判断运动幅度，使得运动幅度的判断结果更准确。

本申请第二方面提供了一种定位初始化装置，包括：获取模块，用于获取预设数量的初始图像，预设数量大于或等于4，且小于预设帧数阈值，预设帧数阈值由执行设备的计算能力确定；视觉初始化模块，用于使用预设数量的初始图像进行视觉初始化操作得到视觉初始化结果，视觉初始化结果包括所有初始图像的位姿以及初始地图；联合初始化模块，用于结合视觉初始化结果与惯性传感器信息对定位状态量进行初始化。

本申请第三方面提供了一种电子设备，包括存储器和处理器，处理器用于执行存储器中存储的程序指令，以实现上述定位初始化方法。

本申请第四方面提供了一种计算机可读存储介质，其上存储有程序指令，程序指令被处理器执行时实现上述定位初始化方法。

上述方案，通过根据执行设备的计算能力确定预设帧数阈值，并使用至少四帧且小于预设帧数阈值的初始图像即可完成初始化，相较于一般需要使用大量的初始图像完成初始化的方案而言，前者初始化的速度更快以及降低了初始化的计算量。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，而非限制本申请。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，这些附图示出了符合本申请的实施例，并与说明书一起用于说明本申请的技术方案。

图1是本申请定位初始化方法一实施例的第一流程示意图；

图2是本申请定位初始化方法一实施例中的第二流程示意图；

图3是本申请定位初始化装置一实施例的结构示意图；

图4是本申请电子设备一实施例的结构示意图；

图5是本申请计算机可读存储介质一实施例的结构示意图。

具体实施方式

下面结合说明书附图，对本申请实施例的方案进行详细说明。

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、接口、技术之类的具体细节，以便透彻理解本申请。

本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。此外，本文中的“多”表示两个或者多于两个。另外，本文中术语“至少一种”表示多种中的任意一种或多种中的至少两种的任意组合，例如，包括A、B、C中的至少一种，可以表示包括从A、B和C构成的集合中选择的任意一个或多个元素。

请参阅图1，图1是本申请定位初始化方法一实施例的第一流程示意图。具体而言，可以包括如下步骤：

步骤S11：获取第一预设数量帧初始图像，其中，第一预设数量大于或等于4，且小于预设帧数阈值，预设帧数阈值由执行设备的计算能力确定。

获取的第一预设数量帧的初始图像帧可以是任意具有摄像组件的设备拍摄得到，当然还可以是从其他设备中获取到的图像，也可以是经过选帧、调整亮度、分辨率等的图像。其中，其他设备指的是分别利用不同中央处理器才能进行操作的设备。即，初始图像可以是执行定位初始化的设备拍摄得到，也可以是需要定位的设备拍摄后，根据各种通信方式传输给执行定位初始化的设备。本公开实施例以初始图像是执行定位初始化的设备拍摄得到的为例，其中，将执行设备称为目标(以下目标均表示执行设备)。即初始图像帧是目标采集得到。例如，在无人机驾驶过程中，无人机即为本公开实施例所述的目标。或者扫地机器人在工作中，扫地机器人也是本公开实施例所述的目标。

可选地，第一预设数量大于或等于4，例如当第一预设数量等于4时，即通过四帧初始图像即可完成定位所需状态量的初始化。当然，在其他实施例中，第一预设数量还可为5、6及以上。其中，为控制计算量，第一预设数量可小于或等于预设帧数阈值。例如，预设帧数阈值为10，则表明第一预设数量大于或等于4且小于10。其中，这里的预设帧数阈值可以根据目标的计算能力确定，因为预设帧数阈值越大，则执行设备需要处理的初始图像就越多，定位初始化过程中所需计算力就越大。目标的计算能力够强，则预设帧数阈值可相对设置更大，若目标的计算能力较弱，则将预设帧数阈值相对设置小一点，以防止因为目标的计算能力不足，导致初始化耗时过长，或造成目标卡顿等情况出现。可选地，目标的计算能力包括目标每秒能够用于对自身进行定位的时间、每秒执行定位的次数，和/或执行一次定位操作的时间。例如，目标每秒能够用于对自身进行定位的时间有300ms，且每秒执行定位的次数为3次，也就是在100ms以内需要完成定位初始化以及对目标的定位操作，虽然并未每次执行目标的定位操作之前都需要执行定位初始化，但为了保证在执行定位初始化之后有时间执行对目标的定位操作，需要控制定位初始化的时间，假设执行一次定位操作的时间为50ms，则需要在另一50ms完成定位初始化操作，因此，为保证目标能够在50ms内完成定位初始化的条件下，确定预设帧数阈值，其中，以上数字仅为举例。其中，第一预设数量的确定方式参见预设帧数阈值的确定方式，此处不再赘述。

步骤S12：利用第一预设数量帧初始图像进行视觉初始化，得到视觉初始化结果，其中，视觉初始化结果包括初始图像的位姿。

本公开实施例中，在获得第一预设数量的初始图像之后，可将其中一帧初始图像的位置固定，计算其与相邻初始图像之间的相对位置关系。计算相邻初始图像之间的相对位置关系的方式很多，例如，根据对极约束技术获取两初始图像之间的距离和角度。其中对极约束技术主要过程包括提取相邻两初始图像的特征点，并进行匹配，并利用匹配的特征点对计算两初始图像之间的基础矩阵，最后从基础矩阵中分解得到相机的旋转矩阵和平移向量。其中，这里的旋转矩阵即表征了两初始图像帧之间的旋转角度，平移向量则表征了两初始图像之间的距离。当然，计算相邻初始图像之间的相对位置关系的方式还可以是假定两初始图像之间的相对距离，利用两初始图像之间的陀螺仪传感数据进行预积分得到两初始图像之间的相对角度，从而得到两初始图像之间的相对位置关系。以此方法，可以获取到第一预设数量帧中各初始图像之间的相对位置关系。其中，这里的初始图像帧之间的相对位置关系即为初始图像的位姿。可选地，视觉初始化结果还可包括各三维点坐标。具体地，可根据初始图像的位姿，获取各初始图像中特征点即二维点，将各二维点进行三角化即可得到其对应的三维点坐标。一般各三维点可以组成一个简易的初始地图。当计算得到的各初始图像帧之间的相对位置关系以及各三维点的坐标之后，可以认为视觉初始化结束。

步骤S13：结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化。

其中，定位状态量为对目标进行定位所需的状态量。一般定位状态量中会包含有真实尺度的位置以及与重力方向对齐的角度、速度、传感器的偏置量等。但是在视觉初始化中，因为对极约束提供的两初始图像之间的相对距离误差可能较大，或两初始图像帧之间的相对距离使用的是假定的距离，也就是计算的到的初始图像的位置不包含真实的尺度，以及计算得到的角度是否与重力对齐是未知。因此，需要使用连续两帧初始图像之间的传感器的预积分信息去求解尺度、各帧初始图像对应的速度以及重力方向。当然，若目标本身能够提供准确的重力方向，可以认为重力方向已知，不必对重力方向进行求解。其中，求解的方式可以是线性求解也可以是非线性求解。

若各状态量求解成功，则对定位状态量初始化完成。初始化完成之后，可以是用初始化得到的各状态量对目标进行定位。

上述方案，根据执行设备的计算能力确定预设帧数阈值，并通过使用至少四帧且小于预设帧数阈值的初始图像即可完成初始化，相较于一般需要使用大量的初始图像完成初始化的方案而言，前者初始化的速度更快以及降低了初始化的计算量。

一些公开实施例中，获取第一预设数量帧初始图像的方式可以是以下至少一个：一是将每间隔预设时间拍摄得到的图像作为初始图像。例如，预设时间为100ms，当然，在其他实施例中预设时间还可为200ms。即每隔100ms提取一帧拍摄的图像作为初始图像，因为相机拍摄图像的频率固定，因此，在这100ms中间会有很多拍摄的图像不会参与初始化过程，其中，这部分初始图像不会对其进行图像处理，即不会对其进行特征提取和匹配，仅对初始图像进行图像处理，很大程度上减小了初始化过程中图像处理的计算量，同时，因为获取的相邻初始图像之间的时间差增大，使得两初始图像之间的传感数据增多，从而提升初始化过程中尺度计算的准确性。当然，若不考虑计算量的问题，可以对拍摄得到的每帧图像都作为初始图像，并对每帧初始图像进行图像处理。此时预设时间则为相机拍摄图像的频率有关。

二是获取拍摄得到的待定图像，并判断待定图像与第四初始图像之间的第二运动幅度是否大于第二预设幅度，在第二运动幅度大于第二预设幅度的情况下，确定待定图像为初始图像。待定图像指的是经目标拍摄得到但还未进行是否能够成为初始图像判定的图像。其中，第四初始图像为与待定图像之间的时间差最小的初始图像。也就是将拍摄得到的待定图像与最新的一初始图像之间的运动幅度进行比较，若运动幅度较小，则待定图像不会作为初始图像参与初始化，也并不会对其进行图像处理。可选地，运动幅度可以通过视差和/或传感数据进行判断。具体地，判断待定图像与第四初始图像之间的视差是否大于预设视差，在视差大于预设视差的情况下，确定第二运动幅度大于第二预设幅度。视差的判断方式可以是提取待定图像与第四初始图像中的二维点，并进行匹配得到若干个二维点对，将二维点对的视差作为待定图像与第四初始图像之间的视差。可选地，可以将所有二维点对的视差中最高视差与最低视差剔除，计算其余视差的均值，将均值作为待定图像与第四初始图像之间的视差。例如，当目标拍摄的待定图像和第四初始图像中均拍到了月亮，但是因为月亮与目标之间的距离过远，因此，即使目标经过了较大的运动，从月亮上提取的二维点对应的视差也比较小，说明从月亮上提取的二维点不适合作为目标的运动幅度，因此，可以将视差变化最小的视差给剔除。当然，可以不是在所有条件下都将视差最小的剔除，而是当最小的视差比包含该视差的平均视差的小若干时，再将最小的视差剔除。视差最大的剔除方式同理。

或，判断待定图像与第四初始图像之间的惯性传感器的预积分数值是否大于预设积分数值，在预积分数值大于预设积分数值的情况下，确定第二运动幅度大于第二预设幅度。其中，这里的预积分数值可以是速度、距离以及角度，其中，速度为加速度计读数的预积分，而距离为加速度计读数的两次预积分，即速度的预积分，而角度为陀螺仪读数的预积分。其中，可以为速度、距离和角度分别设置对应的预设积分数值，在待定图像和第四初始图像之间的速度、距离以及角度均满足对应的预设积分数值的情况下，则认定第二运动幅度大于第二预设幅度。当然，还可在待定图像与第四初始图像之间的速度、距离以及角度中的任一个大于对应的预设积分数值时，则认定第二运动幅度大于第二预设幅度。

一些公开实施例中，在待定图像与第四初始图像之间的视差大于预设视差且预积分数值大于预设积分数值时，才认定第二运动幅度大于第二预设幅度。

在获取第一预设数量帧的初始图像之后，利用第一预设数量帧初始图像进行视觉初始化。当然，为了保证后续初始化结果的稳定性，判断是否存在相邻初始图像之间的第一运动幅度大于第一预设幅度，若存在，则执行利用第一预设数量帧初始图像进行视觉初始化。若不存在，则认定定位初始化失败。其中，这里的运动幅度可以通过视差和/或预积分数值进行判断。本公开实施例以视差为例，若不存在相邻初始图像之间的视差大于视差阈值要求，则认定定位初始化失败。此处进行定位初始化失败判断的原因在于，若初始图像之间的视差都很小，则很难通过三角化得到各匹配二维点的三维点坐标，以及若三维点数量过少，后续初始化结果的准确度很难得到保证，因此，此时直接认定初始化失败，可以减少因为初始化得到的状态量不准确而造成后续定位不准的问题出现，同时还能节省计算量，尽快开始下一次初始化过程。

利用第一预设数量帧初始图像进行视觉初始化的具体步骤可包括：以其中一帧初始图像作为参考图像，并确定至少一帧第一初始图像与参考图像之间的第一相对位置关系。其中，第一初始图像为除参考图像以外的初始图像。也即是可以将其中一帧的初始图像位置固定，然后计算第一初始图像与参考图像之间的相对位置关系。其中，相对位置关系包括距离和/或相对角度。

其中，参考图像的选择方式可以是，确定预设第一数量帧中的各相邻初始图像之间运动幅度，确定最大运动幅度对应的两帧初始图像，并将拍摄时间最早的一帧初始图像作为参考图像。其中，这里运动幅度的也同样可以是经过视差和/或传感数据进行判断。本公开实施例选择通过视差来确定参考图像。具体地，选择相邻初始图像中视差最大的两帧中的拍摄时间更早的一帧作为参考图像。其中，本公开实施例以第一初始图像为参考图像的相邻帧且具体为上述最大视差对应的另一初始图像帧为例。当然，在其他实施例中，第一初始图像帧还可包括参考图像帧的另一相邻帧。可选地，将参考图像的位置暂时固定在世界坐标系的原点，然后再确定各初始图像之间的相对位置关系，从而确定得到的相对位置关系即为在世界坐标系下的位置关系。

确定第一初始图像与参考图像之间的第一相对位置关系的方式可以是将第一初始图像与参考图像之间的距离确定为预设距离值。也就是在不知道真实尺度的情况下，给第一初始图像和参考图像之前的距离确定为不包括真实尺度的距离，就是假定一个距离，该距离可能并不是真正准确的距离。角度的确定方式有多种，例如，基于第一初始图像与参考图像之间的惯性传感器预积分得到第一初始图像与参考图像之间的相对角度，或以对极约束的方式获取第一初始图像与参考图像之间的相对角度。其中，这里的惯性传感器预积分主要指的是陀螺仪读数的预积分，因为陀螺仪测量的是角速度，通过预积分即可得到相对角度。通过对极约束获取相对角度的方式如上所述此处不再赘述。

通过预设距离值以及通过惯性传感器/对极约束的方式可以获取参考图像和第一初始图像之间的距离和位置即可获得参考图像和第一初始图像之间的相对位置关系。

获得第一相对位置关系之后，根据第一相对位置关系，确定至少一帧第一初始图像与参考图像中的共同观测二维点对应的第一三维点的位置信息。具体地，对第一初始图像与参考图像中共同观测到的二维点进行三角化得到各二维点对应的第一三维点的位置信息。在获得第一三维点的位置信息之后，利用第一三维点的位置信息，确定第二初始图像与参考图像之间的第二相对位置关系，其中，第二初始图像为第一初始图像和参考图像之外的初始图像。第一初始图像、第二初始图像和参考图像的数量总和为第一预设数量。其中，这里的第二相对位置关系包括角度和位置，因为参考图像设置为了世界坐标系下的原点，因此，这里的角度和位置可以认为是第二初始图像在世界坐标系下的角度和位置。具体地，找到第一三维点在各第二初始图像中对应的二维点，之后适用通常使用的方式求解各初始图像的位置即可，此处不再赘述。

通过先确定部分初始图像的位置，再确定其余初始图像的位置加快了视觉初始化的速度。

在利用第一三维点的位置信息，确定第二初始图像与参考图像之间的第二相对位置关系之后，确定存在于第二预设数量帧初始图像的若干二维点，其中，若干二维点不对应于第一三维点。本公开实施例中，第二预设数量大于或等于3。即该若干二维点同时存在于三帧初始图像中，且未经过三角化得到对应的第一三维点。获取确定的二维点对应的第二三维点的位置信息。具体获取第二三维点的位置信息的方式包括三角化。之后，优化初始图像的位姿、第一三维点的位置信息以及第二三维点的位置信息中的至少一种。其中，这里的初始图像的位姿包括第一初始图像和第二初始图像相对于参考图像的位置和角度。优化的方式包括将各帧初始图像的位姿以及第一三维点和第二三维点的位置信息联合在一起做非线性优化。其中，非线性优化可以同时对各帧初始图像的位姿以及第一、第二三维点的位置进行优化，得到最终视觉初始化的结果。

通过确定至少存在与第二预设数量帧初始图像上且未与第一三维点对应的二维点，并获取其对应的三维点信息，再联合第一三维点和第二三维点的位置信息以及初始图像的位姿联合优化，使得视觉初始化的结果更准确。

在得到最终视觉初始化的结果之后，判断视觉初始化处理是否成功。其中，结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化是在确定视觉初始化处理成功之后执行的。其中，确定视觉初始化处理成功的步骤包括以下至少一个，一是确定第一三维点的数量大于第一预设阈值。若第一三维点的数量过少，计算得到的第二初始图像与参考图像之间的第二相对位置关系就不太准确，为减小将错误的状态量用于定位的情况出现，这种情况下直接认定视觉初始化失败。二是确定深度为正的三维点的数量大于第二预设阈值，其中，三维点包括第一三维点和第二三维点。深度为正指的是计算得到的三维点位于目标的前方。根据成像原理，目标背后也即是摄像头背后的三维点是拍摄不到的，若三角化得到的三维点位于目标的后面，三维点的深度为负，则说明出现了问题。所以真正有用的只有深度为正的三维点，若深度为正的三维点数量过少则对于计算得到的各初始图像的位姿误差可能较大，使得最终状态量的初始化结果误差也较大。三是确定三维点在初始图像上的平均重投影误差小于或等于第三预设阈值。若重投影误差较大，则说明各初始图像的位姿计算有较大问题，无法继续进行后续的初始化过程。

通过判断视觉初始化成功之后再结合视觉初始化结果与惯性传感器信息，对定位所需状态量进行初始化，减少视觉初始化失败的结果与惯性传感器之间错误结合造成计算资源的浪费。

其中，在确定视觉初始化成功之后，结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化。具体地，将各初始图像之间的惯性传感器信息与视觉初始化结果进行对齐处理，得到经初始化的定位状态量。其中，状态量信息包括尺度。对齐处理中使用的惯性传感器的偏置量为预设值。当然，若目标的摄像头为双目、多目摄像头，或目标具有深度获取传感器，可以直接得到各三维点的深度，即尺度可以直接根据初始图像帧确定，无需通过视觉初始化结果与惯性传感器信息对齐求解尺度。将惯性传感器的偏置量的预设值可以是预先标定、上一次定位时的结果、或零。其中，惯性传感器的偏置量包括加速度计偏置量以及陀螺仪偏置量。对于惯性传感器的偏置量的确定方式可以是，判断是否存在预先标定或上一次定位时的结果，若存在，则使用预先标定或上一次定位时的结果，若不存在，则将预设值取零。因为陀螺仪的偏置量一般较小相对于陀螺仪读数而言，可以忽略不计，故预设值可以为0，而加速度计的偏置量与尺度和重力朝向等信息的关联度较大，若给加速度计偏置量较大的自由度反而可能导致尺度和重力朝向求解失败，因此，给加速度计的偏置量设置为0，可以减少状态量之间的相互影响。

其中，对齐处理中使用的公式可以是预设运动学方程。一般地，在检测到目标处于非静止状态时，定位参数包括

首先，定义一组需要求解的定位参数，

其中，预设运动学方程如下：

一些公开实施例中，对齐处理的方式可以采用传感数据的预积分信息与视觉初始化的定位状态量线性求解，其中，视觉初始化的定位状态量包括三维点及其重投影误差，还可以采用这两类信息的非线性优化。因此，预设运动学方程的构建方式并不限于上述运动学方程。

其中，求解得到的尺度和速度和/或重力方向为使得上述预设运动学方程组误差最小的一组解。也就是将各个预设运动学方程的误差求和最小的一组解。通过将使得预设运动学方程组误差最小的一组解作为求解得到的尺度与速度和/或重力方向，使得求解得到的尺度与速度相对其他解而言更准确，后续定位的结果更准确。

在求解得到尺度和速度和/或重力方向之后，需要判断状态量的初始化是否成功。具体判断方式包括以下至少一个步骤：一是确定经初始化的至少部分定位状态量处于第一预设范围。具体地，确定计算得到的尺度是否在第一预设范围。第一预设范围的确定方式包括根据目标标定的最快运动速度与预积分时间得到可能的最大尺度，若对齐处理求解得到的尺度大于该最大尺度，则认定尺度不在第一预设范围。当然，部分状态量还可包括速度，若速度超过目标标定的最快运动速度则认定速度不处于第一预设范围。其中，若存在一者或多者不处于第一预设范围，则可认定状态量初始化失败。

二是基于经初始化的定位状态量确定传感数据，且确定传感数据处于第二预设范围。其中，传感数据包括重力大小、惯性传感器的偏置量中的一者或多者。具体地，基于定位所需的状态初始化结果确定传感数据的方式包括根据初始化的结果重新计算重力、陀螺仪偏置量和/或加速度计偏置量中的一者或多者。并判断重新计算得到的重力、陀螺仪偏置量和/或加速度计偏置量是否位于对应的第二预设范围。其中，重力大小的第二预设范围为9.81N上下若干牛顿。这里若干牛顿可根据定位精度需求自行设定。陀螺仪偏置量的第二预设范围为市面上一般陀螺仪偏置量上下浮动的若干，加速度计偏置量的第二预设范围确定方式为市面上一般加速度计偏置量上下浮动的若干，也可按定位精度需求设定，一般若干为一倍以内。

通过判断初始化得到的状态量是否在合理的范围，判断初始化是否成功，减少将错误的初始化结果用于定位的情况出现。

将求解到的尺度以及目标的速度作为初始化的尺度和速度的结果。其中，其余定位参数包括角度、陀螺仪和加速度计偏置量可以直接使用初始化之前的预设值，当然，可以使用预设值，例如出厂标定值或上一次定位使用的值。也即是，本公开实施例中，不会初始化陀螺仪和加速度计的偏置量。

在定位状态量进行初始化成功之后，调整各初始图像在世界坐标系下的位置。具体地，确定第三初始图像在世界坐标系的预设位置，其中，第三初始图像为满足预设要求的初始图像。其中，预设要求为拍摄时间最早。即，将第一预设数量中拍摄时间最早的初始图像的位置调整至世界坐标系的预设位置。预设位置可以是原点。基于第三初始图像所确定的位置以及相邻初始图像之间的相对位置关系，调整除第三初始图像以外的其余初始图像在世界坐标系下的位置。此时调整其余初始图像位置的过程实际上就是整体初始图像平移的过程。

通过固定一初始图像在世界坐标下的位置，能够确定其余初始图像在世界坐标系下的位置。

一些公开实施例中，在各状态量初始化成功之后，设置状态量的权重。具体包括以下至少一个步骤：将第三初始图像的位置的权重设置为第一权重。将第三初始图像的航向角的权重设置为第二权重。将所述惯性传感器中的陀螺仪的偏置量的权重设置为第三权重。将第一预设数量帧初始图像的重力方向、速度以及所述惯性传感器中的加速度计的偏置量对应的权重分别设置为第四权重、第五权重以及第六权重。其中，第一权重大于或等于第一预设权重，第二权重大于或等于第二预设权重，第三权重大于或等于第三预设权重，第六权重大于或等于第六预设权重，第四权重小于或等于第四预设权重，第五权重小于或等于第五预设权重。通过这种方式使得第三初始图像的位姿、第三初始图像的航向角、惯性传感器的陀螺仪的偏置量、加速度计的偏置量分别具有较大的权重，而重力方向以及速度分别具有较小的权重。本公开实施例中，可根据第一确定方式确定各状态量对应的权重。第一确定方式中，各状态量的权重与各状态量之间的误差之间的关系为，状态量的权重的倒数*该状态量的单位＝该状态量的误差。例如，将陀螺仪的偏置量对应的第三预设权重设置为10

一些公开实施例中，第一权重、第二权重以及第三权重均大于第四权重、第五权重以及第六权重。其中，可根据第二确定方式确定各状态量对应的权重。第二确定方式可以是根据各状态量对应的误差确定对应的权重。即根据误差与权重之间的对应关系确定，例如，某状态量的误差小于第一预设误差，则设置该状态量的权重为与第一预设误差对应的第一预设权重，例如，第一预设权重为0.9，若某状态量的误差大于第一预设误差且小于第二预设误差，则设置该状态量的权重为第二预设权重，例如，第二预设权重为0.7……根据此种方法，分别确定第一权重至第六权重。通过设置第一权重至第六权重的大小关系，使得后续在对目标进行定位或者对初始化后的状态量进行更新的过程中，能够更多的依靠不确定度低的状态量去更新不确定度高的状态量。

一些公开实施例中，在确定定位初始化失败的情况下，删除第一预设数量帧中拍摄时间最早的初始图像以及初始化过程中的至少部分处理结果。其中，初始化过程中的至少部分处理结果包括各初始图像帧之间的相对位置、各三维点的位置信息、尺度、速度中的一者或多者。删除拍摄时间最早的初始图像之后，重新获取一初始图像，使得下次仍然有第一预设数量帧的初始图像用于初始化。

通过初始化失败之后，删除拍摄时间最早的初始图像使得下次可以使用新的初始图像进行初始化，提高初始化的成功率。

其中，确定定位初始化失败的方式包括以下至少一步：一是若视觉初始化失败，则确定定位初始化失败。其中若确定视觉初始化不成功，则认定其失败。二是若对定位状态量进行初始化失败，则确定定位初始化失败。其中确定定位状态量进行初始化不成功，则认定其失败。三是如上述，在执行利用第一预设数量帧初始图像进行视觉初始化之前，判断是否存在相邻所述初始图像之间的第一运动幅度大于第一预设幅度；若存在，则执行利用所述第一预设数量帧初始图像进行视觉初始化，否则确定定位初始化失败。若运动幅度过小，则对应的三维点数量可能过少，得到的视觉初始化结果可能不太准确，直接认定定位初始化失败以减小计算量。

其中，可使用定位初始化之后的定位状态量对目标进行定位，其中，这里的定位可以是根据目标拍摄得到的图像确定目标与图像中包含的各物体之间的物理位置关系，也可以是利用目标上一次的定位信息，以及当前拍摄到的图像综合确定目标的位置，从而实现对目标的定位以及目标运动轨迹的跟踪。

为更好地说明本申请所提出的技术方案，通过参见图2，图2是本申请定位初始化方法一实施例中的第二流程示意图。如图2所示：

步骤S11：获取第一预设数量帧初始图像。其中，获取第一预设数量帧初始图像的方式如上述，此处不再赘述。

步骤S14：判断是否存在相邻初始图像之间的第一运动幅度大于第一预设幅度。若不存在，则执行步骤S15：认定定位初始化失败。如存在，则执行步骤S12：利用第一预设数量帧初始图像进行视觉初始化，得到视觉初始化结果。其中，具体利用第一预设数量帧初始图像进行视觉初始化，得到视觉初始化结果的方式如上述，此处不再赘述。

执行步骤S16：判断视觉初始化是否成功。若不成功，则执行步骤S15。若成功，则执行步骤S13：结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化。其中，具体结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化的方式如上述，此处不再赘述。

执行步骤S17：判断定位状态量进行初始化是否成功。若不成功，则执行步骤S15。若成功，则执行步骤S18：认定定位初始化成功。

上述方案，通过使用至少四帧且小于预设帧数阈值的初始图像即可完成初始化，相较于一般需要使用大量的初始图像完成初始化的方案而言，前者初始化的速度更快以及降低了初始化的计算量。

其中，定位初始化方法的执行主体可以是定位初始化装置，例如，定位初始化方法可以由终端设备或服务器或其它处理设备执行，其中，终端设备可以为：虚拟现实头盔，增强现实眼镜、无人车、移动机器人、扫地机器人、飞行设备、用户设备(User Equipment，UE)、移动设备、用户终端、终端、蜂窝电话、无绳电话、个人数字处理(Personal DigitalAssistant，PDA)、手持设备、计算设备、车载设备、可穿戴设备等同时具有图像传感器和惯性传感器的设备。在一些可能的实现方式中，该定位初始化方法可以通过处理器调用存储器中存储的计算机可读指令的方式来实现。

请参阅图3，图3是本申请定位初始化装置一实施例的结构示意图。定位初始化装置30包括：获取模块31、视觉初始化模块32以及联合初始化模块33。获取模块31用于获取第一预设数量帧初始图像，其中，第一预设数量大于或等于4，且小于预设帧数阈值，预设帧数阈值由执行设备的计算能力确定；视觉初始化模块32用于利用第一预设数量帧初始图像进行视觉初始化，得到视觉初始化结果，其中，视觉初始化结果包括初始图像的位姿；联合初始化模块33用于结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化。

上述方案，通过使用至少四帧且小于预设帧数阈值的初始图像即可完成初始化，相较于一般需要使用大量的初始图像完成初始化的方案而言，前者初始化的速度更快以及降低了初始化的计算量。

一些公开实施例中，联合初始化模块33结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化，包括：将各初始图像之间的惯性传感器信息与视觉初始化结果进行对齐处理，得到经初始化的定位状态量信息，其中，对齐处理中所使用的惯性传感器的偏置量为预设值。

上述方案，通过将惯性传感器的偏置量定为预设值，减小因为惯性传感器的偏置量与重力和尺度过度耦合造成对尺度和/或重力的进行错误的估值的情况出现，从而降低错误的偏置量对初始化稳定性造成的影响。

一些公开实施例中，视觉初始化模块32利用预设数量的初始图像进行视觉初始化操作得到视觉初始化结果，包括：以其中一帧初始图像作为参考图像，并确定至少一帧第一初始图像与参考图像之间的第一相对位置关系，其中，第一初始图像为除参考图像以外的初始图像；根据第一相对位置关系，确定至少一帧第一初始图像与参考图像中的共同观测二维点对应的第一三维点的位置信息；利用第一三维点的位置信息，确定第二初始图像与参考图像之间的第二相对位置关系，其中，第二初始图像为除第一初始图像和参考图像之外的初始图像，第一初始图像、第二初始图像和参考图像的数量总和为第一预设数量。

上述方案，通过先确定部分初始图像的位置，再确定其余初始图像的位置加快了视觉初始化的速度。

一些公开实施例中，相对位置关系包括距离和/或相对角度；视觉初始化模块32确定至少一帧第一初始图像与参考图像之间的第一相对位置关系，包括：将第一初始图像与参考图像之间的距离确定为预设距离值；和/或，基于第一初始图像与参考图像之间的惯性传感器预积分，得到第一初始图像与参考图像之间的相对角度，或以对极约束的方式获取第一初始图像与参考图像之间的相对角度。

上述方案，通过预设距离值以及通过惯性传感器/对极约束的方式可以获取参考图像和第一初始图像之间的距离和位置即可获得参考图像和第一初始图像之间的相对位置关系。

一些公开实施例中，视觉初始化模块32在利用第一三维点的位置信息，确定第二初始图像与参考图像之间的第二相对位置关系之后，视觉初始化模块32还用于：确定存在于第二预设数量帧初始图像的若干二维点，其中，若干二维点不对应于第一三维点；获取确定的二维点对应的第二三维点的位置信息；优化初始图像的位姿、第一三维点的位置信息和第二三维点的位置信息中的至少一种。

上述方案，通过确定至少存在与第二预设数量帧初始图像上且未与第一三维点对应的二维点，并获取其对应的三维点信息，再联合第一三维点和第二三维点的位置信息以及初始图像的位姿联合优化，使得视觉初始化的结果更准确。

一些公开实施例中，结合视觉初始化结果与惯性传感器信息，对定位状态量进行初始化是在确定视觉初始化处理成功之后执行的；视觉初始化模块32还用于执行以下至少一个步骤，以确定视觉初始化处理成功：确定第一三维点的数量大于第一预设阈值；确定深度为正的三维点的数量大于第二预设阈值，其中，三维点包括第一三维点和第二三维点；确定三维点在初始图像上的平均重投影误差小于或等于第三预设阈值。

上述方案，通过判断视觉初始化成功之后再结合视觉初始化结果与惯性传感器信息，对定位所需状态量进行初始化，减少视觉初始化失败的结果与惯性传感器之间错误结合造成计算资源的浪费。

一些公开实施例中，联合初始化模块33还用于执行以下至少一个步骤，以确定状态量的初始化成功：确定经初始化的至少部分状态量处于第一预设范围；基于经初始化的定位状态量确定传感数据，且确定传感数据处于第二预设范围；其中，传感数据包括重力大小、惯性传感器的偏置量中的一者或多者。

上述方案，通过判断初始化得到的定位状态量是否在合理的范围，判断初始化是否成功，减少将错误的初始化结果用于定位的情况出现。

一些公开实施例中，在定位状态量进行初始化成功之后，联合初始化模块33还用于：确定第三初始图像在世界坐标系的预设位置，其中，第三初始图像为满足预设要求的初始图像；基于第三初始图像所确定的位置以及相邻初始图像之间的相对位置关系，调整除第三初始图像以外的其余初始图像在世界坐标系上的位置。

上述方案，通过固定一初始图像在世界坐标下的位置，能够确定其余初始图像在世界坐标系下的位置。

一些公开实施例中，预设要求为拍摄时间最早；和/或，联合初始化模块33还用于执行以下至少一个步骤，以设置状态量的权重：将第三初始图像的位置的权重设置为第一权重；将第三始图像的航向角的权重设置为第二权重；将惯性传感器中的陀螺仪的偏置量的权重设置为第三权重；将第一预设数量帧初始图像的重力方向、速度以及惯性传感器中的加速度计的偏置量对应的权重分别设置为第四权重、第五权重以及第六权重；其中，状态量的权重越大，对应的不确定度越低。

上述方案，因为拍摄得到的首帧初始图像的位置以及其航向角在世界坐标系上的位置不可知，将首帧初始图像的位置设置为世界坐标系的预设位置以及其航向角并给予较大的权重，使得后续通过该位置调整得到其余初始图像的位置更可靠。

一些公开实施例中，第一权重、第二权重以及第三权重均大于第四权重、第五权重以及第六权重。

上述方案，通过确定第一权重至第六权重之间的大小关系，使得后续在对目标进行定位或者对初始化后的状态量进行更新的过程中，能够更多的依靠不确定度低的状态量去更新不确定度高的状态量。

一些公开实施例中，联合初始化模块33还用于执行以下至少一个步骤：若视觉初始化失败，则确定定位初始化失败；若对定位状态量进行初始化失败，则确定定位初始化失败。在执行利用第一预设数量帧初始图像进行视觉初始化之前，判断是否存在相邻初始图像之间的第一运动幅度大于第一预设幅度；若存在，则执行利用第一预设数量帧初始图像进行视觉初始化，否则确定定位初始化失败。

上述方案，若运动幅度过小，则对应的三维点数量可能过少，得到的视觉初始化结果可能不太准确，直接认定定位初始化失败以减小计算量。

一些公开实施例中，联合初始化模块33还用于：在确定定位初始化失败的情况下，删除第一预设数量帧中拍摄时间最早的初始图像以及初始化过程中的至少部分处理结果。

上述方案，通过初始化失败之后，删除拍摄时间最早的初始图像使得下次可以使用新的初始图像进行初始化，提高初始化的成功率。

一些公开实施例中，获取模块31获取第一预设数量帧初始图像，包括以下至少一个步骤：将每间隔预设时间拍摄得到的图像作为初始图像；获取拍摄得到的待定图像，并判断待定图像与第四初始图像之间的第二运动幅度是否大于第二预设幅度，在第二运动幅度大于第二预设幅度的情况下，确定待定图像为初始图像；其中，第四初始图像为与待定图像之间的时间差最小的初始图像。

上述方案，固定获取初始图像的间隔时间或通过运动幅度获取初始图像，而是将拍摄得到的所有图像帧作为初始图像，能够降低初始化过程中的计算量。

一些公开实施例中，获取模块31判断待定图像与第四初始图像之间的第二运动幅度是否大于第二预设幅度，包括：判断待定图像与第四初始图像之间的视差是否大于预设视差，在视差大于预设视差的情况下，确定第二运动幅度大于第二预设幅度；或，判断待定图像与第四初始图像之间的惯性传感器的预积分数值是否大于预设积分数值，在预积分数值大于预设积分数值的情况下，确定第二运动幅度大于第二预设幅度。

上述方案，通过视差和/或预积分数值判断运动幅度，使得运动幅度的判断结果更准确。

上述方案，通过使用至少四帧且小于预设帧数阈值的初始图像即可完成初始化，相较于一般需要使用大量的初始图像完成初始化的方案而言，前者初始化的速度更快以及降低了初始化的计算量。

请参阅图4，图4是本申请电子设备一实施例的结构示意图。电子设备40包括存储器41和处理器42，处理器42用于执行存储器41中存储的程序指令，以实现上述任一定位初始化方法实施例中的步骤。在一个具体的实施场景中，电子设备40可以包括但不限于：移动机器人、手持移动设备、扫地机器人、无人车、虚拟现实头盔、增强现实眼镜飞行设备、微型计算机、台式电脑、服务器等其他同时具有图像传感器和惯性传感器模组的设备，此外，电子设备40还可以包括笔记本电脑、平板电脑等移动设备，在此不做限定。

具体而言，处理器42用于控制其自身以及存储器41以实现上述任一定位初始化方法实施例中的步骤。处理器42还可以称为CPU(Central Processing Unit，中央处理单元)。处理器42可能是一种集成电路芯片，具有信号的处理能力。处理器42还可以是通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(ApplicationSpecific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable GateArray,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。另外，处理器42可以由集成电路芯片共同实现。

上述方案，通过使用至少四帧且小于预设帧数阈值的初始图像即可完成初始化，相较于一般需要使用大量的初始图像完成初始化的方案而言，前者初始化的速度更快以及降低了初始化的计算量。

请参阅图5，图5是本申请计算机可读存储介质一实施例的结构示意图。计算机可读存储介质50存储有能够被处理器运行的程序指令51，程序指令51用于实现上述任一定位初始化方法实施例中的步骤。

上述方案，通过根据执行设备的计算能力确定预设帧数阈值，并使用至少四帧且小于预设帧数阈值的初始图像即可完成初始化，相较于一般需要使用大量的初始图像完成初始化的方案而言，前者初始化的速度更快以及降低了初始化的计算量。

在一些实施例中，本公开实施例提供的装置具有的功能或包含的模块可以用于执行上文方法实施例描述的方法，其具体实现可以参照上文方法实施例的描述，为了简洁，这里不再赘述。

上文对各个实施例的描述倾向于强调各个实施例之间的不同之处，其相同或相似之处可以互相参考，为了简洁，本文不再赘述。

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

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。