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

权利要求书

1.一种SLAM后端优化方法，其特征在于，包括：

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

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

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

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

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

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

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

基于所述第二海塞伯格矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量，其中，所述位姿增量包括位姿平移增量和位姿旋转增量；

基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第一旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第一旋转矩阵，获得第二旋转矩阵，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据；

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

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

响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，输出所述第二旋转矩阵和更新后的各地图点位置数据，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。

2.如权利要求1所述的SLAM后端优化算法，还包括：

响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，根据预设公式对所述海塞伯格矩阵的线性权值进行更新，以获得所述海塞伯矩阵的第二更新线性权值；

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

基于所述第二更新的海塞伯格矩阵和所述优化项，获得所述待优化地图上的所有地图点位置增量以及关键帧位姿增量，其中，所述关键帧的位姿增量包括位姿平移增量和位姿旋转增量；

基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第二旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第二旋转矩阵，获得第三旋转矩阵，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据；

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

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

响应于确定所述第三鲁棒核误差小于第二鲁棒核误差，输出所述第三旋转矩阵和更新后的各地图点位置数据，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据。

3.如权利要求1所述的SLAM后端优化方法，其中，对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值的过程用公式表示为：其中，Diagonal为计算矩阵的对角元，H为海塞伯格矩阵，Max为计算H矩阵最大对角元，Tau为0.00001，/为标量乘运算。

4.如权利要求2所述的SLAM后端优化方法，其中，用以更新海塞伯格矩阵的线性权值的该预设公式为：其中，Nstep为λ的步进系数，λ为海塞伯格矩阵更新的线性权值，/为标量乘运算。