一种基于视觉惯性里程计处理的方法和相关装置
技术领域
本申请涉及数据处理技术领域，尤其涉及一种基于视觉惯性里程计处理的方法和相关装置。
背景技术
视觉惯性里程计是融合视觉相机和惯性测量单元(英文：Inertial measurementunit，缩写：IMU)两个传感器的测量数据实现跟踪定位的算法。由于视觉相机与IMU之间可以互补，基于视觉惯性里程计的跟踪定位具有良好的精度和鲁棒性，广泛应用于机器人、无人机、自动驾驶、增强现实和虚拟现实等领域。
基于视觉惯性里程计的跟踪定位所采用的基于滤波的方法、基于非线性优化的方法、松耦合类方法以及紧耦合类方法等各种方法中，紧耦合类方法中视觉惯性系统(英文：Visual-Inertial System，缩写：VINS)方法实现跟踪定位精度和鲁棒性最好。VINS方法通过最小化视觉重投影误差和IMU测量误差来求解状态信息的最优解来实现跟踪定位。
发明人经过研究发现，当视觉相机的视觉失效时，VINS方法只能依赖IMU的测量数据来实现跟踪定位，当视觉失效时间较长时，跟踪定位容易存在较大的累积误差，即使在视觉失效恢复后，也会因为前述较大的累积误差导致跟踪定位不稳定或者跟踪定位失败。
发明内容
有鉴于此，本申请实施例提供一种基于视觉惯性里程计处理的方法和相关装置，减少视觉丢失时只能依赖IMU的测量数据来实现跟踪定位存在的累积误差，使得视觉失效恢复后跟踪定位较为稳定。
第一方面，本申请实施例提供了一种基于视觉惯性里程计处理的方法，所述方法包括：
基于视觉相机的第i个视觉帧、第i-1个视觉帧和所述第i个视觉帧之间惯性测量单元IMU的测量数据进行数据预处理，获得第i个视觉惯性帧；所述视觉惯性帧包括视觉帧和IMU本体帧，i为正整数；
对所述第i个视觉惯性帧进行基于滑动窗口的视觉惯性紧耦合优化，获得所述第i个视觉惯性帧的状态信息；
若所述第i个视觉惯性帧中视觉帧为视觉失效恢复帧，基于所述第i个视觉惯性帧进行视觉惯性重新对准；
若视觉惯性重新对准成功，基于视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息，获得中间的视觉惯性帧中IMU本体帧的速度信息；
基于所述中间的视觉惯性帧中IMU本体帧的速度信息，调整相应的视觉惯性帧的状态信息。
可选的，所述基于视觉相机的第i个视觉帧、第i-1个视觉帧和所述第i个视觉帧之间惯性测量单元IMU的测量数据进行数据预处理，获得第i个视觉惯性帧，包括：
获得所述视觉相机的第i个视觉帧；对所述第i-1个视觉帧和所述第i个视觉帧之间IMU的测量数据进行预积分处理，获得第i个IMU本体帧；所述第i个IMU本体帧与所述第i个视觉帧的帧率和时间戳相同；
组合所述第i个视觉帧和所述第i个IMU本体帧，获得所述第i个视觉惯性帧。
可选的，所述第i个视觉惯性帧的状态信息至少包括所述第i个IMU本体帧到初始视觉帧的平移、所述第i个IMU本体帧在初始视觉帧坐标系下的速度、所述第i个IMU本体帧到所述初始视觉帧的旋转；或者，所述第i个视觉惯性帧的状态信息包括所述第i个IMU本体帧到世界帧的平移、所述第i个IMU本体帧在世界帧坐标系下的速度、所述第i个IMU本体帧到所述世界帧的旋转。
可选的，所述第i个视觉惯性帧中视觉帧为视觉失效恢复帧是指当所述第i个视觉惯性帧中视觉帧不符合预设视觉失效条件时，所述第i个视觉惯性帧中视觉帧符合预设视觉恢复条件；其中，所述预设视觉失效条件具体为视觉帧的跟踪特征点数量小于预设值，所述预设视觉恢复条件具体为上一个视觉惯性帧中视觉帧符合所述预设视觉失效条件。
可选的，所述视觉惯性重新对准成功是指符合预设对准条件，所述预设对准条件包括以下一种或多种：
视觉失效恢复帧的数量条件、视觉惯性重新对准后视觉惯性帧中IMU本体帧的速度信息条件、视觉惯性重新对准后视觉惯性帧中IMU本体帧的重力加速度信息条件。
可选的，所述基于视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息，获得中间的视觉惯性帧中IMU本体帧的速度信息，包括：
将所述视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、所述视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息作为边界条件约束，建立线性方程组；
求解所述线性方程组，获得所述中间的视觉惯性帧中IMU本体帧的速度信息。
可选的，还包括：
当所述第i个视觉惯性帧中视觉帧符合预设视觉失效条件时，确定所述第i个视觉惯性帧中视觉帧为所述视觉失效帧；
基于所述第i个视觉惯性帧中IMU本体帧和第i-1个视觉惯性帧的状态信息，获得所述第i个视觉惯性帧的新状态信息。
第二方面，本申请实施例提供了一种基于视觉惯性里程计处理的装置，所述装置包括：
第一获得单元，用于基于视觉相机的第i个视觉帧、第i-1个视觉帧和所述第i个视觉帧之间惯性测量单元IMU的测量数据进行数据预处理，获得第i个视觉惯性帧；所述视觉惯性帧包括视觉帧和IMU本体帧，i为正整数；
第二获得单元，用于对所述第i个视觉惯性帧进行基于滑动窗口的视觉惯性紧耦合优化，获得所述第i个视觉惯性帧的状态信息；
对准单元，用于若所述第i个视觉惯性帧中视觉帧为视觉失效恢复帧，基于所述第i个视觉惯性帧进行视觉惯性重新对准；
第三获得单元，用于若视觉惯性重新对准成功，基于视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息，获得中间的视觉惯性帧中IMU本体帧的速度信息；
调整单元，用于基于所述中间的视觉惯性帧中IMU本体帧的速度信息，调整相应的视觉惯性帧的状态信息。
第三方面，本申请实施例提供了一种终端设备，所述终端设备包括处理器以及存储器：
所述存储器用于存储程序代码，并将所述程序代码传输给所述处理器；
所述处理器用于根据所述程序代码中的指令执行上述第一方面任一项所述的基于视觉惯性里程计处理的方法。
第四方面，本申请实施例提供了一种计算机可读存储介质，其特征在于，所述计算机可读存储介质用于存储程序代码，所述程序代码用于执行上述第一方面任一项所述的基于视觉惯性里程计处理的方法。
与现有技术相比，本申请至少具有以下优点：
采用本申请实施例的技术方案，基于视觉相机的第i个视觉帧、第i-1个视觉帧和第i个视觉帧之间惯性测量单元IMU的测量数据进行数据预处理，获得包括视觉帧和IMU本体帧的第i个视觉惯性帧，i为正整数；对第i个视觉惯性帧进行基于滑动窗口的视觉惯性紧耦合优化，获得第i个视觉惯性帧的状态信息；若第i个视觉惯性帧中视觉帧为视觉失效恢复帧，基于第i个视觉惯性帧进行视觉惯性重新对准；若视觉惯性重新对准成功，基于视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息，获得中间的视觉惯性帧中IMU本体帧的速度信息；基于中间的视觉惯性帧中IMU本体帧的速度信息，调整相应的视觉惯性帧的状态信息。由此可见，当视觉失效恢复后，通过视觉丢失前后视觉惯性帧中IMU本体帧的速度信息，调整视觉丢失时视觉惯性帧的状态信息，可以减少只能依赖IMU的测量数据来实现跟踪定位存在的累积误差，使得视觉失效恢复后跟踪定位较为稳定。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1为本申请实施例中一种应用场景所涉及的系统框架示意图；
图2为本申请实施例提供的一种基于视觉惯性里程计处理的方法的流程示意图；
图3为本申请实施例提供的一种基于视觉惯性里程计处理的装置的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
现阶段，基于视觉惯性里程计的跟踪定位通常采用VINS方法，VINS方法是指通过最小化视觉重投影误差和IMU测量误差来求解状态信息。当视觉相机的视觉失效时，VINS方法只能依赖IMU的测量数据来实现跟踪定位，当视觉失效时间较长时，跟踪定位容易存在较大的累积误差，即使在视觉失效恢复后，也会因为前述较大的累积误差导致跟踪定位不稳定或者跟踪定位失败。
为了解决这一问题，在本申请实施例中，基于视觉相机的第i个视觉帧、第i-1个视觉帧和第i个视觉帧之间惯性测量单元IMU的测量数据进行数据预处理，获得包括视觉帧和IMU本体帧的第i个视觉惯性帧，i为正整数；对第i个视觉惯性帧进行基于滑动窗口的视觉惯性紧耦合优化，获得第i个视觉惯性帧的状态信息；若第i个视觉惯性帧中视觉帧为视觉失效恢复帧，基于第i个视觉惯性帧进行视觉惯性重新对准；若视觉惯性重新对准成功，基于视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息，获得中间的视觉惯性帧中IMU本体帧的速度信息；基于中间的视觉惯性帧中IMU本体帧的速度信息，调整相应的视觉惯性帧的状态信息。可见，当视觉失效恢复后，通过视觉丢失前后视觉惯性帧中IMU本体帧的速度信息，调整视觉丢失时视觉惯性帧的状态信息，可以减少只能依赖IMU的测量数据来实现跟踪定位存在的累积误差，使得视觉失效恢复后跟踪定位较为稳定。
举例来说，本申请实施例的场景之一，可以是应用到如图1所示的场景中，该场景包括视觉相机101、IMU102和视觉惯性处理单元103，视觉相机101、IMU102和视觉惯性处理单元103可以组成视觉惯性里程计系统，视觉相机与IMU相对位置及角度固定。视觉相机101输出的视觉帧至视觉惯性处理单元103，IMU102输出测量数据至视觉惯性处理单元103；视觉惯性处理单元103执行本申请实施例提供的实施方式，使得视觉失效恢复后跟踪定位较为稳定。
首先，在上述应用场景中，虽然将本申请实施例提供的实施方式的动作描述由视觉惯性处理单元103执行；但是本申请实施例在执行主体方面不受限制，只要执行了本申请实施例提供的实施方式所公开的动作即可。
其次，上述场景仅是本申请实施例提供的一个场景示例，本申请实施例并不限于此场景。
下面结合附图，通过实施例来详细说明本申请实施例中基于视觉惯性里程计处理的方法和相关装置的具体实现方式。
示例性方法
参见图2，示出了本申请实施例中一种基于视觉惯性里程计处理的方法的流程示意图。在本实施例中，所述方法例如可以包括以下步骤：
步骤201：基于视觉相机的第i个视觉帧、第i-1个视觉帧和所述第i个视觉帧之间惯性测量单元IMU的测量数据进行数据预处理，获得第i个视觉惯性帧；所述视觉惯性帧包括视觉帧和IMU本体帧。
在本申请实施例中，视觉惯性里程计系统包括视觉相机、IMU和视觉惯性处理单元，视觉相机将视觉帧输入视觉惯性处理单元，IMU将测量数据输入视觉惯性处理单元；视觉惯性处理单元需要预处理输入的数据，已获得包括视觉帧和IMU本体帧的视觉惯性帧。
其中，视觉相机例如可以是单目相机、双目相机或RGBD相机，IMU为6轴惯性传感器或9轴惯性传感器。需要预先确定视觉惯性里程计系统的视觉相机、IMU及其环境参数，比如，视觉相机与IMU的外参，视觉相机的内参，IMU的内参，重力加速度信息等。
作为一种示例，手机包括视觉惯性里程计系统，手机的相机作为视觉相机、手机的惯性传感器作为IMU、手机的处理器中运行视觉惯性处理单元。作为另一种示例，VR头盔包括视觉惯性里程计系统，VR头盔的相机作为视觉相机、VR头盔的惯性传感器作为IMU、VR头盔的处理器中运行视觉惯性处理单元。
具体地，步骤201中对第i个视觉帧、第i-1个视觉帧和所述第i个视觉帧之间IMU的测量数据的数据预处理过程实际上是：先将第i-1个视觉帧和第i个视觉帧之间IMU的测量数据进行预积分处理，以得到与第i个视觉帧的帧率和时间戳相同的IMU本体帧，作为第i个IMU本体帧；再将第i个视觉帧和第i个IMU本体帧组合，形成第i个视觉惯性帧。因此，在本申请实施例一种可选的实施方式中，所述步骤201例如可以包括以下步骤：
步骤A：获得所述视觉相机的第i个视觉帧；对所述第i-1个视觉帧和所述第i个视觉帧之间IMU的测量数据进行预积分处理，获得第i个IMU本体帧；所述第i个IMU本体帧与所述第i个视觉帧的帧率和时间戳相同。
作为一种示例，第i个视觉帧为视觉相机拍摄的第i个视觉图像，视觉相机是单目相机，第i个视觉图像为VGA分辨率的RGB图像；视觉相机是RGBD相机，第i个视觉图像包括像素点一一对应的一个VGA分辨率的RGB图像和一个VGA分辨率的深度图像。第i-1个视觉帧为ci-1，其时间戳为ti-1，第i个视觉帧为ci，其时间戳为ti；对时间段[ti-1，ti]内IMU的测量数据进行预积分处理，预积分处理得到的第i个IMU本体帧与第i个视觉帧ci的时间戳是对齐的，第i个IMU本体帧为bi。如果对每两个连续的视觉帧均按照此方法进行预积分处理，则预积分处理得到的IMU本体帧的帧率与视觉帧的帧率是一致的。第i个IMU本体帧为bi包括预积分处理得到的位移增量
步骤B：组合所述第i个视觉帧和所述第i个IMU本体帧，获得所述第i个视觉惯性帧。
步骤202：对所述第i个视觉惯性帧进行基于滑动窗口的视觉惯性紧耦合优化，获得所述第i个视觉惯性帧的状态信息。
在本申请实施例中，在步骤201获得第i个视觉惯性帧之后，需要对第i个视觉惯性帧做基于滑动窗口的视觉惯性紧耦合优化，得到第i个视觉惯性帧的状态信息。其中，基于滑动窗口的视觉惯性紧耦合优化例如可以采用VINS、OKVIS或者VI ORB-SLAM等方法，本申请实施例对此不做具体限定。
在本申请实施例一种可选的实施方式中，由于基于滑动窗口的视觉惯性紧耦合优化在初始化阶段需要进行视觉惯性初始对准，因此，在视觉惯性初始对准完成后，所述第i个视觉惯性帧的状态信息至少包括所述第i个IMU本体帧到初始视觉帧的平移、所述第i个IMU本体帧在初始视觉帧坐标系下的速度、所述第i个IMU本体帧到所述初始视觉帧的旋转；或者，所述第i个视觉惯性帧的状态信息至少包括所述第i个视觉惯性帧的状态信息包括所述第i个IMU本体帧到世界帧的平移、所述第i个IMU本体帧在世界帧坐标系下的速度、所述第i个IMU本体帧到所述世界帧的旋转。
作为一种示例，在上述示例的基础上，初始视觉帧为c0，第i个视觉惯性帧的状态信息为
步骤203：若所述第i个视觉惯性帧中视觉帧为视觉失效恢复帧，基于所述第i个视觉惯性帧进行视觉惯性重新对准。
在本申请实施例中，由于现有技术中在视觉相机的视觉失效时，VINS方法只能依赖IMU的测量数据来实现跟踪定位，当视觉失效时间较长时，跟踪定位容易存在较大的累积误差，即使在视觉失效恢复后，也会因为前述较大的累积误差导致跟踪定位不稳定或者跟踪定位失败。因此，在步骤202之后，需要先判断第i个视觉惯性帧中视觉帧是否为视觉失效恢复帧，即，判断第i个视觉惯性帧中视觉帧是否为视觉相机在视觉失效后又重新恢复视觉的视觉帧；在判断得到第i个视觉惯性帧中视觉帧为视觉失效恢复帧时，再进行视觉惯性重新对准。
其中，视觉惯性重新对准例如可以采用VINS、OKVIS或者VI ORB-SLAM的视觉惯性初始化对准方法，可以得到第i个视觉惯性帧中IMU本体帧的速度信息和重力加速度信息。
其中，判断第i个视觉惯性帧中视觉帧是否为视觉失效恢复帧实际上是指先判断第i个视觉惯性帧中视觉帧是否符合预设视觉失效条件，当第i个视觉惯性帧中视觉帧不符合预设视觉失效条件，表示第i个视觉惯性帧中视觉帧是视觉未失效帧；此情况下再判断第i个视觉惯性帧中视觉帧是否符合预设视觉恢复条件，当第i个视觉惯性帧中视觉帧符合预设视觉恢复条件，表示第i个视觉惯性帧中视觉帧是视觉失效恢复帧。因此，在本申请实施例一种可选的实施方式中，所述第i个视觉惯性帧中视觉帧为视觉失效恢复帧是指当所述第i个视觉惯性帧中视觉帧不符合预设视觉失效条件时，所述第i个视觉惯性帧中视觉帧符合预设视觉恢复条件。
具体地，当视觉帧的跟踪特征点数量小于预设值时，表示该视觉帧是视觉失效帧；因此，预设视觉失效条件例如具体可以为视觉帧的跟踪特征点数量小于预设值。在第i个视觉惯性帧中视觉帧不符合该预设视觉失效条件的情况下，即，第i个视觉惯性帧中视觉帧是视觉未失效帧的情况下；第i-1个视觉惯性帧中视觉帧的跟踪特征点数量小于预设值时，表示第i-1个视觉惯性帧中视觉帧是视觉失效帧，第i个视觉惯性帧中视觉帧是视觉失效恢复帧，因此，预设视觉恢复条件例如具体可以为上一个视觉惯性帧中视觉帧符合上述预设视觉失效条件。
对应地，当第i个视觉惯性帧中视觉帧符合预设视觉失效条件，表示第i个视觉惯性帧中视觉帧是视觉失效帧；此情况下，需要用过第i个视觉惯性帧中IMU本体帧和第i-1个视觉惯性帧的状态信息计算第i个视觉惯性帧的新状态信息，以替换步骤202中的第i个视觉惯性帧的状态信息。因此，在本申请实施例一种可选的实施方式中，所述方法例如还可以包括以下步骤：
步骤C：当所述第i个视觉惯性帧中视觉帧符合预设视觉失效条件时，确定所述第i个视觉惯性帧中视觉帧为所述视觉失效帧。
步骤D：基于所述第i个视觉惯性帧中IMU本体帧和第i-1个视觉惯性帧的状态信息，获得所述第i个视觉惯性帧的新状态信息。
基于上述示例，步骤D中基于第i个视觉惯性帧中IMU本体帧和第i-1个视觉惯性帧的状态信息，获得第i个视觉惯性帧的新状态信息的计算公式是如下所示：
其中，
步骤204：若视觉惯性重新对准成功，基于视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息，获得中间的视觉惯性帧中IMU本体帧的速度信息。
在本申请实施例中，在步骤203进行视觉惯性重新对准之后，需要先判断视觉惯性重新对准是否成功；在判断得到视觉惯性重新对准成功后，再通过视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、第i个视觉惯性帧中IMU本体帧的速度信息，求取视觉失效前最后一个视觉惯性帧与视觉惯性重新对准成功的视觉惯性帧之间的视觉惯性帧(称为中间的视觉惯性帧)中IMU本体帧的速度信息，以便减小视觉丢失时只能依赖IMU的测量数据来实现跟踪定位存在的累积误差。
其中，判断视觉惯性重新对准是否成功实际上是判断是否符合预设对准条件，符合预设对准条件表示视觉惯性重新对准成功。视觉惯性重新对准成功通常需要满足一定的视觉失效恢复帧的数量、视觉惯性重新对准的视觉惯性帧中IMU本体帧的速度信息和/或视觉惯性重新对准的视觉惯性帧中IMU本体帧的重力加速度信息。因此，在本申请实施例一种可选的实施方式中，所述视觉惯性重新对准成功是指符合预设对准条件，所述预设对准条件包括以下一种或多种：视觉失效恢复帧的数量条件、视觉惯性重新对准后视觉惯性帧中IMU本体帧的速度信息条件、视觉惯性重新对准后视觉惯性帧中IMU本体帧的重力加速度信息条件。
例如，视觉失效恢复帧的数量条件可以是包括当前视觉惯性帧中视觉帧在内的视觉失效恢复帧的数量大于数值a，视觉惯性重新对准后视觉惯性帧中IMU本体帧的速度信息条件可以是视觉惯性重新对准后视觉惯性帧中IMU本体帧的速度的模值小于数值b，视觉惯性重新对准后视觉惯性帧中IMU本体帧的重力加速度信息条件可以是视觉惯性重新对准后视觉惯性帧中IMU本体帧的重力加速度的模值与预先确定的重力加速度的模值的偏差小于数值c。
在步骤204具体实施时，首先，将视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息作为边界条件约束，建立线性方程组；然后，求取中间的视觉惯性帧中IMU本体帧的速度信息对应的最小二乘解，以得到具有较少累积误差的中间的视觉惯性帧中IMU本体帧的速度信息。因此，在本申请实施例一种可选的实施方式中，所述步骤204例如可以包括以下步骤：
步骤E：将所述视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、所述视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息作为边界条件约束，建立线性方程组；
例如，假设从视觉失效前最后一个视觉惯性帧到视觉惯性重新对准成功的第i个视觉惯性帧中间一共有m个视觉惯性帧，记视觉失效之前的最后一个视觉惯性帧为第n个视觉惯性帧，视觉惯性重新对准成功的视觉惯性帧为第n+m+1个视觉惯性帧，则步骤E中建立的线性方程组如下所示：
步骤F：求解所述线性方程组，获得所述中间的视觉惯性帧中IMU本体帧的速度信息。
步骤205：基于所述中间的视觉惯性帧中IMU本体帧的速度信息，调整相应的视觉惯性帧的状态信息。
在本申请实施例中，在步骤204获得中间的视觉惯性帧中IMU本体帧的速度信息之后，由于中间的视觉惯性帧中IMU本体帧的速度信息具有较少累积误差，需要根据该中间的视觉惯性帧中IMU本体帧的速度信息，调整相应的视觉惯性帧的状态信息，以便减少视觉丢失时只能依赖IMU的测量数据来实现跟踪定位存在的累积误差，使得视觉失效恢复后跟踪定位较为稳定。
通过本实施例提供的各种实施方式，基于视觉相机的第i个视觉帧、第i-1个视觉帧和第i个视觉帧之间惯性测量单元IMU的测量数据进行数据预处理，获得包括视觉帧和IMU本体帧的第i个视觉惯性帧，i为正整数；对第i个视觉惯性帧进行基于滑动窗口的视觉惯性紧耦合优化，获得第i个视觉惯性帧的状态信息；若第i个视觉惯性帧中视觉帧为视觉失效恢复帧，基于第i个视觉惯性帧进行视觉惯性重新对准；若视觉惯性重新对准成功，基于视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、第i个视觉惯性帧中IMU本体帧的速度信息，获得中间的视觉惯性帧中IMU本体帧的速度信息；基于中间的视觉惯性帧中IMU本体帧的速度信息，调整中间的视觉惯性帧的状态信息和第i个视觉惯性帧的状态信息。由此可见，当视觉失效恢复后，通过视觉丢失前后视觉惯性帧中IMU本体帧的速度信息，调整视觉丢失时视觉惯性帧的状态信息，可以减少只能依赖IMU的测量数据来实现跟踪定位存在的累积误差，使得视觉失效恢复后跟踪定位较为稳定。
示例性装置
参见图3，示出了本申请实施例中一种基于视觉惯性里程计处理的装置的结构示意图。在本实施例中，所述装置例如具体可以包括：
第一获得单元301，用于基于视觉相机的第i个视觉帧、第i-1个视觉帧和所述第i个视觉帧之间惯性测量单元IMU的测量数据进行数据预处理，获得第i个视觉惯性帧；所述视觉惯性帧包括视觉帧和IMU本体帧；
第二获得单元302，用于对所述第i个视觉惯性帧进行基于滑动窗口的视觉惯性紧耦合优化，获得所述第i个视觉惯性帧的状态信息；
对准单元303，用于若所述第i个视觉惯性帧中视觉帧为视觉失效恢复帧，基于所述第i个视觉惯性帧进行视觉惯性重新对准；
第三获得单元304，用于若视觉惯性重新对准成功，基于视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息，获得中间的视觉惯性帧中IMU本体帧的速度信息；
调整单元305，用于基于所述中间的视觉惯性帧中IMU本体帧的速度信息，调整相应的视觉惯性帧的状态信息。
在本申请实施例一种可选的实施方式中，所述第一获得单元301包括：
第一获得子单元，用于获得所述视觉相机的第i个视觉帧；对所述第i-1个视觉帧和所述第i个视觉帧之间IMU的测量数据进行预积分处理，获得第i个IMU本体帧；所述第i个IMU本体帧与所述第i个视觉帧的帧率和时间戳相同；
第二获得子单元，用于组合所述第i个视觉帧和所述第i个IMU本体帧，获得所述第i个视觉惯性帧。
在本申请实施例一种可选的实施方式中，所述第i个视觉惯性帧的状态信息至少包括所述第i个IMU本体帧到初始视觉帧的平移、所述第i个IMU本体帧在初始视觉帧坐标系下的速度、所述第i个IMU本体帧到所述初始视觉帧的旋转；或者，所述第i个视觉惯性帧的状态信息包括所述第i个IMU本体帧到世界帧的平移、所述第i个IMU本体帧在世界帧坐标系下的速度、所述第i个IMU本体帧到所述世界帧的旋转。
在本申请实施例一种可选的实施方式中，所述第i个视觉惯性帧中视觉帧为视觉失效恢复帧是指当所述第i个视觉惯性帧中视觉帧不符合预设视觉失效条件时，所述第i个视觉惯性帧中视觉帧符合预设视觉恢复条件；其中，所述预设视觉失效条件具体为视觉帧的跟踪特征点数量小于预设值，所述预设视觉恢复条件具体为上一个视觉惯性帧中视觉帧符合所述预设视觉失效条件。
在本申请实施例一种可选的实施方式中，所述视觉惯性重新对准成功是指符合预设对准条件，所述预设对准条件包括以下一种或多种：
视觉失效恢复帧的数量条件、视觉惯性重新对准后视觉惯性帧中IMU本体帧的速度信息条件、视觉惯性重新对准后视觉惯性帧中IMU本体帧的重力加速度信息条件。
在本申请实施例一种可选的实施方式中，所述第三获得单元304包括：
建立子单元，用于将所述视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、所述视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息作为边界条件约束，建立线性方程组；
第三获得子单元，用于求解所述线性方程组，获得所述中间的视觉惯性帧中IMU本体帧的速度信息。
在本申请实施例一种可选的实施方式中，所述装置还包括：
确定单元，用于当所述第i个视觉惯性帧中视觉帧符合预设视觉失效条件时，确定所述第i个视觉惯性帧中视觉帧为所述视觉失效帧；
第五获得单元，用于基于所述第i个视觉惯性帧中IMU本体帧和第i-1个视觉惯性帧的状态信息，获得所述第i个视觉惯性帧的新状态信息。
通过本实施例提供的各种实施方式，基于视觉相机的第i个视觉帧、第i-1个视觉帧和第i个视觉帧之间惯性测量单元IMU的测量数据进行数据预处理，获得包括视觉帧和IMU本体帧的第i个视觉惯性帧，i为正整数；对第i个视觉惯性帧进行基于滑动窗口的视觉惯性紧耦合优化，获得第i个视觉惯性帧的状态信息；若第i个视觉惯性帧中视觉帧为视觉失效恢复帧，基于第i个视觉惯性帧进行视觉惯性重新对准；若视觉惯性重新对准成功，基于视觉失效前最后一个视觉惯性帧中IMU本体帧的速度信息、视觉惯性重新对准成功的视觉惯性帧中IMU本体帧的速度信息，获得中间的视觉惯性帧中IMU本体帧的速度信息；基于中间的视觉惯性帧中IMU本体帧的速度信息，调整相应的视觉惯性帧的状态信息。由此可见，当视觉失效恢复后，通过视觉丢失前后视觉惯性帧中IMU本体帧的速度信息，调整视觉丢失时视觉惯性帧的状态信息，可以减少只能依赖IMU的测量数据来实现跟踪定位存在的累积误差，使得视觉失效恢复后跟踪定位较为稳定。
此外，本申请实施例还提供了一种终端设备，所述终端设备包括处理器以及存储器：
所述存储器用于存储程序代码，并将所述程序代码传输给所述处理器；
所述处理器用于根据所述程序代码中的指令执行上述方法实施例所述的基于视觉惯性里程计处理的方法。
本申请实施例还提供了一种计算机可读存储介质，其特征在于，所述计算机可读存储介质用于存储程序代码，所述程序代码用于执行上述方法实施例所述的基于视觉惯性里程计处理的方法。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述，仅是本申请的较佳实施例而已，并非对本申请作任何形式上的限制。虽然本申请已以较佳实施例揭露如上，然而并非用以限定本申请。任何熟悉本领域的技术人员，在不脱离本申请技术方案范围情况下，都可利用上述揭示的方法和技术内容对本申请技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例。因此，凡是未脱离本申请技术方案的内容，依据本申请的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本申请技术方案保护的范围内。
