[发明专利]SLAM后端优化方法、优化装置和电子设备

说明书

一种SLAM后端优化方法、优化装置和电子设备，其中，该优化方法通过选择特定的旋转矩阵替代四元数关键帧相机位姿直接地参与到SLAM后端优化的各个计算阶段，以减少SLAM后端优化的计算量。这样，所述SLAM后端优化方法及其装置能够及时地完成对关键帧相机位姿和地图点位置的优化，从而确保关键帧相机位姿和地图点位置误差不会累积，并保证最终的地图重建和即时定位的质量。

技术领域

本申请涉及SLAM领域，尤其涉及SLAM后端优化方法、优化装置和电子设备。

背景技术

在机器视觉中，SLAM(Simultaneous Localization and Mapping)技术用于面向未知环境进行即时定位和地图重建。SLAM是一个完整的系统，有许多分支模块组成，目前主流的技术方案为基于图的SLAM方案。

基于图的SLAM包括前端和后端两个部分，其中，前端主要负责地图的构建，后端负责地图的优化。后端优化的质量极大程度影响着最终SLAM的地图重建质量和即时定位的精度。

目前基于图的SLAM后端优化通常采用G2O框架(A General Framework for GraphOptimization)。在利用G2O框架进行SLAM后端优化过程中，需要对大量的关键帧相机位姿和地图点位置进行优化。然而，这一过程计算代价过于庞大，导致无法及时地完成对关键帧相机位姿和地图点位置的优化，进而导致关键帧相机位姿和地图点位置误差不断累积，影响最终的地图重建和即时定位的质量。

因此，对于一种能够减少计算量的SLAM后端优化方法的需求是迫切的。

申请内容

本申请的主要目的在于提供一种SLAM后端优化方法、优化装置和电子设备，其中，所述SLAM后端优化方法以旋转矩阵替代四元数作为数据格式表示关键帧相机位姿，以实现减少SLAM后端优化计算量的目的。

本申请的主要目的在于提供一种SLAM后端优化方法、优化装置和电子设备，其中，所述关键帧相机位姿采用旋转矩阵表示，该旋转矩阵与四元数数据格式不同，但物理意义相同，从而利用旋转矩阵表示关键帧相机位姿参与后端优化，不仅能够降低计算量，而且能够不影响SLAM后端优化之效果。

本申请的另一目的在于提供一种SLAM后端优化方法、优化装置和电子设备，其中，通过所述SLAM后端优化方法能够有效地降低后端优化的计算量，以加速SLAM后端优化的工作。

通过下面的描述，本申请的其它优势和特征将会变得显而易见，并可以通过权利要求书中特别指出的手段和组合得到实现。

为实现上述至少一目的或优势，本申请提供一种SLAM后端优化方法，其包括：

对待优化的地图进行预处理，以将所述待优化的地图上的地图点位置数据和关键帧相机位姿数据设定为图的顶点，以及，将各顶点之间的关系设定为所述图中各顶点之间的边，其中，所述关键帧相机位姿数据基于第一旋转矩阵表示；

基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第一反投影误差；

基于所述第一反投影误差、信息矩阵、鲁棒核参数和鲁棒核函数，获得第一鲁棒核误差；

基于所述第一旋转矩阵对所述图中各条边的两顶点进行处理，以获得所述图中各条边的两顶点之间的雅阁比矩阵；

基于所述第一反投影误差、所述信息矩阵和所述雅阁比矩阵，获得海塞伯矩阵和优化项；

对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值；

基于所述第一更新线性权值对所述海塞伯矩阵进行更新，以获得第二海塞伯格矩阵；