基于视觉惯性的SLAM方法、装置、存储介质和设备与流程
本公开涉及无线定位技术领域，具体地，涉及一种基于视觉惯性的slam方法、装置、存储介质和设备。
背景技术：
随着终端技术的不断发展，机器人已经进入到许多技术领域，以帮助人们完成各种作业任务。对于需要在工作过程中不断移动的机器人来说，为了更好的完成作业任务，需要获得准确的定位。机器人利用各种信息采集设备以采集机器人所处物理环境的外部信息，由此进行slam(英文：simultaneouslocalizationandmapping，中文：即时定位与地图构建)。外部信息通常包括视觉信息(例如图像)和运动信息(加速度、方向等)两种，需要结合两种信息以实现slam。现有技术中，对视觉信息和运动信息的结合，通常有两种处理方式：卡尔曼滤波算法和非线性优化算法。其中，卡尔曼滤波算法实现简单，计算效率高，但定位精度比较低，难以适用于需要准确定位的应用场景，非线性优化算法通过迭代来实现，精度较高，但计算量大，计算效率低，难以适用于需要实时定位的应用场景。
技术实现要素：
本公开的目的是提供一种基于视觉惯性的slam方法、装置、存储介质和设备，用以解决现有技术中存在的难以兼顾slam的计算效率和定位精度的问题。
为了实现上述目的，根据本公开实施例的第一方面，提供一种基于视觉惯性的slam方法，所述方法包括：
将第一时刻图像采集单元采集的图像信息和惯性测量单元imu采集的运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿；
根据所述图像信息和所述初始位姿，确定初始局部地图；
按照预设的非线性优化算法对所述初始位姿和所述初始局部地图进行优化，以获取优化位姿和优化局部地图；
根据所述优化位姿和所述优化局部地图，更新目标初始位姿和目标初始局部地图，所述目标初始位姿为将第二时刻所述图像采集单元采集的图像信息和所述imu采集的运动信息按照所述扩展卡尔曼滤波算法进行融合，获取的初始位姿，所述目标初始局部地图为根据所述第二时刻所述图像采集单元采集的图像信息和所述目标初始位姿确定的初始局部地图，所述第二时刻为获取所述优化位姿和所述优化局部地图的时刻。
可选地，所述将第一时刻图像采集单元采集的图像信息和惯性测量单元imu采集的运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿，包括：
将所述运动信息和历史位姿作为imu运动模型的输入，以获取所述imu运动模型输出的预测位姿，所述历史位姿为将第三时刻所述图像采集单元采集的图像信息和所述imu采集的运动信息按照所述扩展卡尔曼滤波算法进行融合，获取的初始位姿，所述第三时刻为所述第一时刻之前的时刻；
按照预设的图像处理算法处理所述图像信息，以获取所述图像信息中包括的视觉约束关系；
按照所述视觉约束关系对所述预测位姿进行更新，以获取所述初始位姿，所述视觉约束关系包括：重投影误差最小，或光度误差最小。
可选地，所述根据所述图像信息和所述初始位姿，确定初始局部地图，包括：
将所述图像信息中满足预设条件的像素点确定为特征点；
根据所述特征点和所述初始位姿进行三维重建，以获取所述初始局部地图。
可选地，所述按照预设的非线性优化算法对所述初始位姿和所述初始局部地图进行优化，以获取优化位姿和优化局部地图，包括：
当所述图像信息为关键帧时，根据所述初始位姿、所述初始局部地图和所述第一时刻之前的预设数量个关键位姿信息建立目标函数，所述目标函数包括所述imu的测量残差和所述图像采集单元的测量残差，所述关键位姿信息为所述第一时刻之前所述图像采集单元采集的历史图像信息为关键帧时，所述历史图像信息中包括的位姿信息；
按照预设算法对所述目标函数进行迭代，以获取在所述目标函数最小的条件下对应的所述优化位姿和所述优化局部地图。
可选地，所述根据所述优化位姿和所述优化局部地图，更新目标初始位姿和目标初始局部地图，包括：
将所述目标初始局部地图更新为所述优化局部地图；
当所述视觉约束关系为重投影误差最小时，将所述优化位姿和第二时刻之前的预设数量个关键位姿信息按照卡尔曼滤波算法进行融合，以将所述目标初始位姿更新为所述卡尔曼滤波算法输出的位姿，所述关键位姿信息为所述第二时刻之前所述图像采集单元采集的历史图像信息为关键帧时，所述历史图像信息中包括的位姿信息；
当所述视觉约束关系为光度误差最小时，获取所述初始位姿与所述优化位姿之间的偏移量；
根据所述偏移量更新所述目标初始位姿。
可选地，所述根据所述优化位姿和所述优化局部地图，更新目标初始位姿和目标初始局部地图，还包括：
将更新后的所述目标初始局部地图存入全局地图。
根据本公开实施例的第二方面，提供一种基于视觉惯性的slam装置，所述装置包括：
融合模块，用于将第一时刻图像采集单元采集的图像信息和惯性测量单元imu采集的运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿；
确定模块，用于根据所述图像信息和所述初始位姿，确定初始局部地图；
优化模块，用于按照预设的非线性优化算法对所述初始位姿和所述初始局部地图进行优化，以获取优化位姿和优化局部地图；
更新模块，用于根据所述优化位姿和所述优化局部地图，更新目标初始位姿和目标初始局部地图，所述目标初始位姿为将第二时刻所述图像采集单元采集的图像信息和所述imu采集的运动信息按照所述扩展卡尔曼滤波算法进行融合，获取的初始位姿，所述目标初始局部地图为根据所述第二时刻所述图像采集单元采集的图像信息和所述目标初始位姿确定的初始局部地图，所述第二时刻为获取所述优化位姿和所述优化局部地图的时刻。
可选地，所述融合模块包括：
预测子模块，用于将所述运动信息和历史位姿作为imu运动模型的输入，以获取所述imu运动模型输出的预测位姿，所述历史位姿为将第三时刻所述图像采集单元采集的图像信息和所述imu采集的运动信息按照所述扩展卡尔曼滤波算法进行融合，获取的初始位姿，所述第三时刻为所述第一时刻之前的时刻；
图像处理子模块，用于按照预设的图像处理算法处理所述图像信息，以获取所述图像信息中包括的视觉约束关系；
更新子模块，用于按照所述视觉约束关系对所述预测位姿进行更新，以获取所述初始位姿，所述视觉约束关系包括：重投影误差最小，或光度误差最小。
可选地，所述确定模块包括：
确定子模块，用于将所述图像信息中满足预设条件的像素点确定为特征点；
建图子模块，用于根据所述特征点和所述初始位姿进行三维重建，以获取所述初始局部地图。
可选地，所述优化模块包括：
建立子模块，用于当所述图像信息为关键帧时，根据所述初始位姿、所述初始局部地图和所述第一时刻之前的预设数量个关键位姿信息建立目标函数，所述目标函数包括所述imu的测量残差和所述图像采集单元的测量残差，所述关键位姿信息为所述第一时刻之前所述图像采集单元采集的历史图像信息为关键帧时，所述历史图像信息中包括的位姿信息；
迭代子模块，用于按照预设算法对所述目标函数进行迭代，以获取在所述目标函数最小的条件下对应的所述优化位姿和所述优化局部地图。
可选地，所述更新模块包括：
地图更新子模块，用于将所述目标初始局部地图更新为所述优化局部地图；
位姿更新子模块，用于当所述视觉约束关系为重投影误差最小时，将所述优化位姿和第二时刻之前的预设数量个关键位姿信息按照卡尔曼滤波算法进行融合，以将所述目标初始位姿更新为所述卡尔曼滤波算法输出的位姿，所述关键位姿信息为所述第二时刻之前所述图像采集单元采集的历史图像信息为关键帧时，所述历史图像信息中包括的位姿信息；
所述位姿更新子模块，还用于当所述视觉约束关系为光度误差最小时，获取所述初始位姿与所述优化位姿之间的偏移量，并根据所述偏移量更新所述目标初始位姿。
可选地，所述地图更新子模块，还用于将更新后的所述目标初始局部地图存入全局地图。
根据本公开实施例的第三方面，提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现第一方面提供的基于视觉惯性的slam方法的步骤。
根据本公开实施例的第四方面，提供一种电子设备，包括：
存储器，其上存储有计算机程序；
处理器，用于执行所述存储器中的所述计算机程序，以实现第一方面提供的基于视觉惯性的slam方法的步骤。
通过上述技术方案，本公开首先将图像采集单元和imu在第一时刻采集的图像信息和运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿，再根据图像信息和初始位姿来确定初始局部地图，之后按照预设的非线性优化算法对所述初始位姿和所述初始局部地图进行优化，以获取优化位姿和优化局部地图，最后根据所述优化位姿和所述优化局部地图，更新第二时刻对应的目标初始位姿和目标初始局部地图。通过将扩展卡尔曼滤波算法的输出作为非线性优化算法的输入，并利用非线性优化算法的输出来修正扩展卡尔曼滤波算法的输出，从而提高slam的计算效率和定位精度。
本公开的其他特征和优点将在随后的具体实施方式部分予以详细说明。
附图说明
附图是用来提供对本公开的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本公开，但并不构成对本公开的限制。在附图中：
图1是根据一示例性实施例示出的一种基于视觉惯性的slam方法的流程图；
图2是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图；
图3是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图；
图4是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图；
图5是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图；
图6是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图；
图7是根据一示例性实施例示出的一种基于视觉惯性的slam装置的框图；
图8是根据一示例性实施例示出的另一种基于视觉惯性的slam装置的框图；
图9是根据一示例性实施例示出的另一种基于视觉惯性的slam装置的框图；
图10是根据一示例性实施例示出的另一种基于视觉惯性的slam装置的框图；
图11是根据一示例性实施例示出的另一种基于视觉惯性的slam装置的框图；
图12是根据一示例性实施例示出的一种电子设备的框图。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本公开相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本公开的一些方面相一致的装置和方法的例子。
在介绍本公开提供的基于视觉惯性的slam方法、装置、存储介质与设备之前，首先对本公开各个实施例所涉及应用场景进行介绍。该应用场景可以是任一种需要slam的终端(例如：机器人)，该终端上可以设置有两种信息采集设备，包括：图像采集单元和imu(英文：inertialmeasurementunit，中文：惯性测量单元)。其中，图像采集单元例如可以是摄像头或相机，imu中可以包括：陀螺仪、加速度计等传感器，图像采集单元和imu可以固定在一起(即图像采集单元的与imu所在的位置一致)，以确保图像采集单元采集到的图像信息与imu采集的运动信息具有一致性。
图1是根据一示例性实施例示出的一种基于视觉惯性的slam方法的流程图，如图1所示，该方法包括以下步骤：
步骤101，将第一时刻图像采集单元采集的图像信息和惯性测量单元imu采集的运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿。
步骤102，根据图像信息和初始位姿，确定初始局部地图。
举例来说，首先获取图像采集单元在第一时刻采集的终端所处物理环境的图像信息，和imu在第一时刻采集的终端对应的运动信息，运动信息例如可以包括：终端的方位、位置、速度、偏置量等。其中，能够通过运动信息中包含的终端通过惯性测量来预测位姿，再根据图像信息中包含的视觉约束关系(可以理解为观测方程)来更新预测位姿，以确定终端的位姿。按照扩展卡尔曼滤波算法(英文：extendedkalmanfiltering，缩写：ekf)对图像信息和运动信息进行融合，以获取终端在第一时刻的初始位姿和初始局部地图。扩展卡尔曼滤波算法计算较快，并且cpu内存的占用率较低，因此计算效率高。首先，可以将运动信息作为扩展卡尔曼滤波算法运动方程的输入，对终端的位姿进行粗略的预测，以获取扩展卡尔曼滤波算法运动方程输出的预测位姿。由于imu中包含的传感器通常为消费级别的传感器，误差较大，容易造成漂移，可以进一步根据图像信息中包含的视觉约束关系作为扩展卡尔曼滤波算法的观测方程，对终端的位姿进行更新，从而得到初始位姿。
进一步的，可以按照预设的图像处理方法对图像信息进行处理，以提取图像信息中包含的纹理信息，再结合初始位姿，得到能够反应终端在第一时刻所处局部环境的初始局部地图。需要说明的是，初始局部地图是由第一时刻之前图像采集单元采集的预设数量个关键帧中，能够观测到的路标点构成的局部地图，可以理解为预先设置了一个滑动时间窗口，用于存储第一时刻之前的n-1个关键帧，和第一时刻对应的图像信息。初始局部地图与滑动时间窗口是对应的关系，当滑动时间窗口内的关键帧更新时，初始局部地图也对应更新。
步骤103，按照预设的非线性优化算法对初始位姿和初始局部地图进行优化，以获取优化位姿和优化局部地图。
示例的，由于efk算法的定位精度较低，因此可以在获取初始位姿和初始局部地图后，选择预设的非线性优化算法，对初始位姿和初始局部地图进行ba(英文：bundleadjustment，中文：光束法平差)优化，以得到定位精度更高的优化位姿和优化局部地图。可以理解为，将初始位姿和初始局部地图作为非线性优化算法初始值，能够使非线性优化算法更快收敛(即计算效率更高)，并获得更准确的定位。
例如，可以先根据imu的测量残差(英文：measurementresidual)和图像采集单元的测量残差来建立一个目标函数，将初始位姿和初始局部地图带入目标函数中，再利用非线性优化方法对目标函数进行迭代，直至目标函数达到最佳状态(即最小化目标函数)，此时目标函数确定的位姿和局部地图即为优化位姿和优化局部地图。其中，非线性优化算法可以包括：高斯-牛顿迭代法、lm(英文：levenberg-marquardtalgorithm，中文：莱文贝格－马夸特算法)优化等算法，本公开对非线性优化算法不做具体限定。
步骤104，根据优化位姿和优化局部地图，更新目标初始位姿和目标初始局部地图，目标初始位姿为将第二时刻图像采集单元采集的图像信息和imu采集的运动信息按照扩展卡尔曼滤波算法进行融合，获取的初始位姿，目标初始局部地图为根据第二时刻图像采集单元采集的图像信息和目标初始位姿确定的初始局部地图，第二时刻为获取优化位姿和优化局部地图的时刻。
示例的，在获取了定位精度更高的优化位姿和优化局部地图，可以将优化位姿和优化局部地图反馈至步骤101和步骤102中，以优化步骤101中利用扩展卡尔曼滤波算法确定的目标初始位姿和步骤102中确定的目标初始局部地图，从而提高slam的定位精度。需要说明的是，由于扩展卡尔曼滤波算法和非线性优化算法的计算效率不同，因此步骤101、步骤102和步骤103可以同时执行，即可以理解为步骤101、步骤102和步骤103分别对应两个可以并行执行的线程，例如步骤101和步骤102为前端线程，步骤103为后端线程，前端线程不需要等待后端线程，可以一直执行。前端线程在确定第一时刻对应的初始位姿和初始局部地图后，将初始位姿和初始局部地图发送至后端线程，由后端线程确定优化位姿和优化局部地图，后端线程在第二时刻将优化位姿和优化局部地图反馈给前端线程，来优化前端线程在第二时刻确定的目标初始位姿和目标初始局部地图。
步骤101确定了第一时刻对应的初始位姿和初始局部地图后，可以继续将第一时刻之后下一时刻(可以理解为：下一时刻＝第一时刻+t)图像采集单元和imu采集的图像信息和运动信息进行融合，以获取下一时刻对应的初始位姿和初始局部地图。而步骤102在对第一时刻对应的初始位姿和初始局部地图进行优化，以得到优化位姿和优化局部地图，由于非线性优化算法的迭代过程耗时较长，因此得到优化位姿和优化局部地图的时间已经为第二时刻，第二时刻与第一时刻之间可能存在多个t(通常为几十毫秒)，即第二时刻＝第一时刻+x*t。步骤101一直在执行，因此在第二时刻，将第二时刻图像采集单元和imu采集的图像信息和运动信息进行融合，获取到的第二时刻对应的目标初始位姿和目标初始局部地图。此时，第一时刻对应的初始位姿和初始局部地图已经失效，不具有实际意义，所以可以根据优化位姿和优化局部地图，更新第二时刻对应的目标初始位姿和目标初始局部地图，从而提高定位精度。
综上所述，本公开首先将图像采集单元和imu在第一时刻采集的图像信息和运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿，再根据图像信息和初始位姿来确定初始局部地图，之后按照预设的非线性优化算法对所述初始位姿和所述初始局部地图进行优化，以获取优化位姿和优化局部地图，最后根据所述优化位姿和所述优化局部地图，更新第二时刻对应的目标初始位姿和目标初始局部地图。通过将扩展卡尔曼滤波算法的输出作为非线性优化算法的输入，并利用非线性优化算法的输出来修正扩展卡尔曼滤波算法的输出，从而提高slam的计算效率和定位精度。
图2是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图，如图2所示，步骤101可以通过以下方式来实现：
步骤1011，将运动信息和历史位姿作为imu运动模型的输入，以获取imu运动模型输出的预测位姿，历史位姿为将第三时刻图像采集单元采集的图像信息和imu采集的运动信息按照扩展卡尔曼滤波算法进行融合，获取的初始位姿，第三时刻为第一时刻之前的时刻。
步骤1012，按照预设的图像处理算法处理图像信息，以获取图像信息中包括的视觉约束关系。
步骤1013，按照视觉约束关系对预测位姿进行更新，以获取初始位姿，视觉约束关系包括：重投影误差最小，或光度误差最小。
本公开中的扩展卡尔曼滤波算法可以包括两种：msckf(英文：multi-stateconstraintkalmanfilter，中文：多状态约束卡尔曼滤波器)算法和rovio(英文：robustvisualinertialodometry，中文：鲁棒视觉惯性里程计)算法。两种算法均采用imu运动模型作为扩展卡尔曼滤波算法运动方程，以获取预测位姿，而msckf算法中的视觉约束关系为重投影误差最小，以此作为观测方程，对预测位姿进行更新，rovio算法中的视觉约束关系为光度误差最小以此作为观测方程，对预测位姿进行更新。
图3是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图，如图3所示，步骤102可以包括以下步骤：
步骤1021，将图像信息中满足预设条件的像素点确定为特征点。
步骤1022，根据特征点和初始位姿进行三维重建，以获取初始局部地图。
示例的，初始局部地图的获取方式，可以是先在图像信息中选取满足预设条件的像素点作为特征点。其中，特征点可以是多个(例如；100个)，预设条件例如可以是：与相邻像素点之间的亮度差值大于预设的亮度阈值的点。之后再利用多个时刻相互匹配的特征点和步骤101中确定的初始位姿进行三维重建，以得到一个能够反应终端在第一时刻所处局部环境的，三维的初始局部地图。
以下分别对两种算法的实现做具体说明：
在msckf算法中，即视觉约束关系为重投影误差最小，将步骤1011中获取的位姿信息、运动信息和第一时刻之前的预设数量个关键位姿信息作为imu运动模型的输入(例如可以是第一时刻之前的最新的关键位姿信息)，以获取imu运动模型输出的预测位姿。
举例来说，输入msckf算法待估计的状态向量可以为
需要说明的是，imu运动模型，描述了位姿信息和运动信息之间的约束关系，由于图像采集单元和imu固定在一起，因此图像信息和运动信息具有一致性，例如：加速度计测量得到的加速度对时间二次积分，得到的值应该与位姿信息中的位置相同，陀螺仪测量得到的方位对时间积分，得到的值应该与位姿信息中的方位相同等。
关键位姿信息可以理解某一时刻，图像采集单元采集的图像信息为关键帧时，该时刻图像信息中包括的位姿信息。第一时刻之前的预设数量个关键位姿信息，可以理解为预先设置了一个滑动的时间窗口，用于存储n个位姿信息，包括：第一时刻之前的n-1个关键位姿信息，和第一时刻对应的位姿信息。其中，判断图像信息是否为关键帧的规则，可以是将该图像信息与上一关键帧时刻获取的图像信息进行位姿比较，当位置的变化量超过了预设的距离阈值，和/或方位的变化量超过了预设的角度阈值，那么确定该图像信息为关键帧。
在rovio算法中，即视觉约束关系为光度误差(特征点的光度误差)最小时，将步骤1011中获取的位姿信息、运动信息和路标点作为imu运动模型的输入，以获取imu运动模型输出的预测位姿，路标点为特征点对应在局部地图中的坐标。
举例来说，输入rovio待估计是的状态向量可以为χ＝(r,v,q,bf,bw,c,z,μ0,…,μm,ρ0,…,ρm)，其中，r为imu在全局坐标系下的坐标，v为imu在全局坐标系下的速度，q为imu在全局坐标系下的方位，bf为加速度计的偏置量，bw为陀螺仪的偏置量，c为图像采集单元与imu外参的平移分量，z为图像采集单元与imu外参的旋转分量，μ0,…,μm为m个特征点在图像采集单元坐标系下的bearingvector，ρ0,…,ρm为m个特征点在图像采集单元坐标系下的逆深度参数，逆深度即为距离参数的倒数。将imu运动模型的输出作为预测位姿，由于imu存在漂移，因此预测位姿不够准确，进一步，按照光度误差最小为视觉约束关系，对预测位姿进行更新，得到的位姿即为初始位姿。
图4是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图，如图4所示，步骤103包括：
步骤1031，当图像信息为关键帧时，根据初始位姿、初始局部地图和第一时刻之前的预设数量个关键位姿信息建立目标函数，目标函数包括imu的测量残差和图像采集单元的测量残差，关键位姿信息为第一时刻之前图像采集单元采集的历史图像信息为关键帧时，历史图像信息中包括的位姿信息，还可以包括imu测量获得的速度和偏置量等信息。
步骤1032，按照预设算法对目标函数进行迭代，以获取在目标函数最小的条件下对应的优化位姿和优化局部地图。
对于步骤103来说，对初始位姿和初始局部地图进行优化，需要建立目标函数来进行回环检测，以获取优化位姿和优化局部地图。由于非线性优化算法计算较慢，并且cpu内存的占用率较高，因此，可以对步骤101获取的初始位姿和初始局部地图进行筛选，只针对图像信息为关键帧的情况下，才对初始位姿和初始局部地图进行优化，即前端线程在确定图像信息为关键帧时，才将初始位姿和初始局部地图发送至后端线程，由后端线程确定优化位姿和优化局部地图。当图像信息不为关键帧时，可以理解为第一时刻的图像信息与上一时刻的图像信息之间变化不大，无需对初始位姿和初始局部地图进行优化。通常情况下，步骤101中获取到的两个连续的关键帧的时间间隔比较长(例如100ms)，因此降低了步骤103的计算量，从而进一步提高slam的计算效率和定位精度。
当图像信息为关键帧时，根据初始位姿、初始局部地图和第一时刻之前的预设数量个关键位姿信息建立目标函数。其中，目标函数可以包括imu的测量残差和图像采集单元的测量残差，例如：imu预积分残差和视觉光度误差等。选定目标函数之后，对目标函数按照预设算法(例如：高斯-牛顿迭代法)进行迭代，直至目标函数达到最佳状态(即目标函数最小)，此时目标函数确定的位姿和局部地图即为优化位姿和优化局部地图。
图5是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图，如图5所示，步骤104包括：
步骤1041，将目标初始局部地图更新为优化局部地图。
举例来说，步骤104中可以分为局部地图优化和位姿优化两部分，对于步骤101在第二时刻确定的目标初始局部地图，可以直接用优化局部地图替代目标初始局部地图，对于步骤101在第二时刻确定的目标初始位姿，又可以根据步骤101中扩展卡尔曼滤波算法中视觉约束的不同分为两种处理方式。
步骤1042，当视觉约束关系为重投影误差最小时，将优化位姿和第二时刻之前的预设数量个关键位姿信息按照卡尔曼滤波算法进行融合，以将目标初始位姿更新为卡尔曼滤波算法输出的位姿，关键位姿信息为第二时刻之前图像采集单元采集的历史图像信息为关键帧时，历史图像信息中包括的位姿信息。
举例来说，在利用卡尔曼滤波算法对优化位姿和第二时刻之前的预设数量个关键位姿信息进行融合时，可以是将优化状态量和关键状态量进行融合，其中，优化状态量包括优化位姿，还可以包括经过步骤103优化得到的速度和imu偏置量，关键状态量包括关键位姿信息和对应的速度和imu偏置量。之后将目标初始状态量更新为卡尔曼滤波算法输出的状态量，其中，目标初始状态量包括：目标初始位姿和对应的速度和imu偏置量。
进一步的，对于第二时刻的目标初始状态量的更新，可以包括：使用优化局部地图、第二时刻对应的关键帧和第n-1个关键帧的状态量建立视觉反投影残差和imu预积分残差项，进行非线性迭代优化(例如高斯-牛顿迭代法等)，得到第二时刻高精度的状态量。与后端步骤103中的优化不同之处是，这里只优化第二时刻的状态量，不需优化局部地图和第n-1帧的状态量(因为这两个量是后端步骤103反馈回来的，准确度高)。
步骤1043，当视觉约束关系为光度误差最小时，获取初始位姿与优化位姿之间的偏移量。
步骤1044，根据偏移量更新目标初始位姿。
具体的，在msckf算法中，即视觉约束关系为重投影误差最小，将优化位姿和第二时刻之前的预设数量个关键位姿信息作为卡尔曼滤波算法(英文：kalmanfiltering，缩写：kf)的输入，利用状态协方差矩阵和过程协方差矩阵，获取卡尔曼滤波算法输出的位姿，并将目标初始位姿和对应的协方差矩阵更新为卡尔曼滤波算法输出的位姿和协方差矩阵。其中，第二时刻之前的预设数量个关键位姿信息，可以理解为预先设置了一个滑动的时间窗口，用于存储第二时刻之前n-1个关键位姿信息。
在rovio算法中，即视觉约束关系为光度误差最小时，可以先获取第一时刻对应的初始位姿与优化位姿之间的偏移量，再利用该偏移量修正目标初始位姿。以第一时刻为t、第二时刻为t+m来举例，偏移量xdiff＝txbac*tx'fro，其中，第一时刻对应的初始位姿为tx'fro，优化位姿为txbac。那么更新后的目标初始位姿t+mx＝xdiff*t+mxfro，其中t+mxfro为第二时刻对应的更新前的目标初始位姿。
图6是根据一示例性实施例示出的另一种基于视觉惯性的slam方法的流程图，如图6所示，步骤104还可以包括：
步骤1045，将更新后的目标初始局部地图存入全局地图。
进一步的，在对目标初始局部地图更新之后，还可以将更新后的目标初始局部地图存入全局地图，即根据目标初始局部地图来更新终端上存储的全局地图。例如，可以根据目标初始局部地图所包含的位置信息，用目标初始局部地图替代全局地图中该位置信息指示的区域对应的地图。
综上所述，本公开首先将图像采集单元和imu在第一时刻采集的图像信息和运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿，再根据图像信息和初始位姿来确定初始局部地图，之后按照预设的非线性优化算法对所述初始位姿和所述初始局部地图进行优化，以获取优化位姿和优化局部地图，最后根据所述优化位姿和所述优化局部地图，更新第二时刻对应的目标初始位姿和目标初始局部地图。通过将扩展卡尔曼滤波算法的输出作为非线性优化算法的输入，并利用非线性优化算法的输出来修正扩展卡尔曼滤波算法的输出，从而提高slam的计算效率和定位精度。
图7是根据一示例性实施例示出的一种基于视觉惯性的slam装置的框图，如图7所示，该装置200包括：
融合模块201，用于将第一时刻图像采集单元采集的图像信息和惯性测量单元imu采集的运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿。
确定模块202，用于根据图像信息和初始位姿，确定初始局部地图。
优化模块203，用于按照预设的非线性优化算法对初始位姿和初始局部地图进行优化，以获取优化位姿和优化局部地图。
更新模块204，用于根据优化位姿和优化局部地图，更新目标初始位姿和目标初始局部地图，目标初始位姿为将第二时刻图像采集单元采集的图像信息和imu采集的运动信息按照扩展卡尔曼滤波算法进行融合，获取的初始位姿，目标初始局部地图为根据第二时刻图像采集单元采集的图像信息和目标初始位姿确定的初始局部地图，第二时刻为获取优化位姿和优化局部地图的时刻。
图8是根据一示例性实施例示出的另一种基于视觉惯性的slam装置的框图，如图8所示，融合模块201包括：
预测子模块2011，用于将运动信息和历史位姿作为imu运动模型的输入，以获取imu运动模型输出的预测位姿，历史位姿为将第三时刻图像采集单元采集的图像信息和imu采集的运动信息按照扩展卡尔曼滤波算法进行融合，获取的初始位姿，第三时刻为第一时刻之前的时刻。
图像处理子模块2012，用于按照预设的图像处理算法处理图像信息，以获取图像信息中包括的视觉约束关系。
更新子模块2013，用于按照视觉约束关系对预测位姿进行更新，以获取初始位姿，视觉约束关系包括：重投影误差最小，或光度误差最小。
图9是根据一示例性实施例示出的另一种基于视觉惯性的slam装置的框图，如图9所示，确定模块202包括：
确定子模块2021，用于将图像信息中满足预设条件的像素点确定为特征点。
建图子模块2022，用于根据特征点和初始位姿进行三维重建，以获取初始局部地图。
图10是根据一示例性实施例示出的另一种基于视觉惯性的slam装置的框图，如图10所示，优化模块203包括：
建立子模块2031，用于当图像信息为关键帧时，根据初始位姿、初始局部地图和第一时刻之前的预设数量个关键位姿信息建立目标函数，目标函数包括imu的测量残差和图像采集单元的测量残差，关键位姿信息为第一时刻之前图像采集单元采集的历史图像信息为关键帧时，历史图像信息中包括的位姿信息。
迭代子模块2032，用于按照预设算法对目标函数进行迭代，以获取在目标函数最小的条件下对应的优化位姿和优化局部地图。
图11是根据一示例性实施例示出的另一种基于视觉惯性的slam装置的框图，如图11所示，更新模块204包括：
地图更新子模块2041，用于将目标初始局部地图更新为优化局部地图。
位姿更新子模块2042，用于当视觉约束关系为重投影误差最小时，将优化位姿和第二时刻之前的预设数量个关键位姿信息按照卡尔曼滤波算法进行融合，以将目标初始位姿更新为卡尔曼滤波算法输出的位姿，关键位姿信息为第二时刻之前图像采集单元采集的历史图像信息为关键帧时，历史图像信息中包括的位姿信息。
位姿更新子模块2042，还用于当视觉约束关系为光度误差最小时，获取初始位姿与优化位姿之间的偏移量，并根据偏移量更新目标初始位姿。
进一步的，地图更新子模块2041还可以用于将更新后的目标初始局部地图存入全局地图。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
综上所述，本公开首先将图像采集单元和imu在第一时刻采集的图像信息和运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿，再根据图像信息和初始位姿来确定初始局部地图，之后按照预设的非线性优化算法对所述初始位姿和所述初始局部地图进行优化，以获取优化位姿和优化局部地图，最后根据所述优化位姿和所述优化局部地图，更新第二时刻对应的目标初始位姿和目标初始局部地图。通过将扩展卡尔曼滤波算法的输出作为非线性优化算法的输入，并利用非线性优化算法的输出来修正扩展卡尔曼滤波算法的输出，从而提高slam的计算效率和定位精度。
图12是根据一示例性实施例示出的一种电子设备300的框图。如图12所示，该电子设备300可以包括：处理器301，存储器302。该电子设备300还可以包括多媒体组件303，输入/输出(i/o)接口304，以及通信组件305中的一者或多者。
其中，处理器301用于控制该电子设备300的整体操作，以完成上述的基于视觉惯性的slam方法中的全部或部分步骤。存储器302用于存储各种类型的数据以支持在该电子设备300的操作，这些数据例如可以包括用于在该电子设备300上操作的任何应用程序或方法的指令，以及应用程序相关的数据，例如联系人数据、收发的消息、图片、音频、视频等等。该存储器302可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，例如静态随机存取存储器(staticrandomaccessmemory，简称sram)，电可擦除可编程只读存储器(electricallyerasableprogrammableread-onlymemory，简称eeprom)，可擦除可编程只读存储器(erasableprogrammableread-onlymemory，简称eprom)，可编程只读存储器(programmableread-onlymemory，简称prom)，只读存储器(read-onlymemory，简称rom)，磁存储器，快闪存储器，磁盘或光盘。多媒体组件303可以包括屏幕和音频组件。其中屏幕例如可以是触摸屏，音频组件用于输出和/或输入音频信号。例如，音频组件可以包括一个麦克风，麦克风用于接收外部音频信号。所接收的音频信号可以被进一步存储在存储器302或通过通信组件305发送。音频组件还包括至少一个扬声器，用于输出音频信号。i/o接口304为处理器301和其他接口模块之间提供接口，上述其他接口模块可以是键盘，鼠标，按钮等。这些按钮可以是虚拟按钮或者实体按钮。通信组件305用于该电子设备300与其他设备之间进行有线或无线通信。无线通信，例如wi-fi，蓝牙，近场通信(nearfieldcommunication，简称nfc)，2g、3g或4g，或它们中的一种或几种的组合，因此相应的该通信组件305可以包括：wi-fi模块，蓝牙模块，nfc模块。
在一示例性实施例中，电子设备300可以被一个或多个应用专用集成电路(applicationspecificintegratedcircuit，简称asic)、数字信号处理器(digitalsignalprocessor，简称dsp)、数字信号处理设备(digitalsignalprocessingdevice，简称dspd)、可编程逻辑器件(programmablelogicdevice，简称pld)、现场可编程门阵列(fieldprogrammablegatearray，简称fpga)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述的基于视觉惯性的slam方法。
在另一示例性实施例中，还提供了一种包括程序指令的计算机可读存储介质，该程序指令被处理器执行时实现上述的基于视觉惯性的slam方法的步骤。例如，该计算机可读存储介质可以为上述包括程序指令的存储器302，上述程序指令可由电子设备300的处理器301执行以完成上述的基于视觉惯性的slam方法。
综上所述，本公开首先将图像采集单元和imu在第一时刻采集的图像信息和运动信息按照扩展卡尔曼滤波算法进行融合，以获取初始位姿，再根据图像信息和初始位姿来确定初始局部地图，之后按照预设的非线性优化算法对所述初始位姿和所述初始局部地图进行优化，以获取优化位姿和优化局部地图，最后根据所述优化位姿和所述优化局部地图，更新第二时刻对应的目标初始位姿和目标初始局部地图。通过将扩展卡尔曼滤波算法的输出作为非线性优化算法的输入，并利用非线性优化算法的输出来修正扩展卡尔曼滤波算法的输出，从而提高slam的计算效率和定位精度。
以上结合附图详细描述了本公开的优选实施方式，但是，本公开并不限于上述实施方式中的具体细节，在本公开的技术构思范围内，可以对本公开的技术方案进行多种简单变型，这些简单变型均属于本公开的保护范围。
另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合，为了避免不必要的重复，本公开对各种可能的组合方式不再另行说明。
此外，本公开的各种不同的实施方式之间也可以进行任意组合，只要其不违背本公开的思想，其同样应当视为本公开所公开的内容。
- 还没有人留言评论。精彩留言会获得点赞！