CN111091602A - Slam后端优化方法、优化装置和电子设备 - Google Patents

Slam后端优化方法、优化装置和电子设备 Download PDF

Info

Publication number
CN111091602A
CN111091602A CN201811235928.2A CN201811235928A CN111091602A CN 111091602 A CN111091602 A CN 111091602A CN 201811235928 A CN201811235928 A CN 201811235928A CN 111091602 A CN111091602 A CN 111091602A
Authority
CN
China
Prior art keywords
matrix
rotation
map
pose
increment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201811235928.2A
Other languages
English (en)
Other versions
CN111091602B (zh
Inventor
徐文杰
王城特
钟家跃
张本好
陈远
胡增新
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sunny Optical Zhejiang Research Institute Co Ltd
Original Assignee
Sunny Optical Zhejiang Research Institute Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Sunny Optical Zhejiang Research Institute Co Ltd filed Critical Sunny Optical Zhejiang Research Institute Co Ltd
Priority to CN201811235928.2A priority Critical patent/CN111091602B/zh
Publication of CN111091602A publication Critical patent/CN111091602A/zh
Application granted granted Critical
Publication of CN111091602B publication Critical patent/CN111091602B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T11/00—2D [Two Dimensional] image generation
    • G06T11/003—Reconstruction from projections, e.g. tomography
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Theoretical Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Computing Systems (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Studio Devices (AREA)
  • Image Analysis (AREA)

Abstract

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

Description

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后端优化方法，其包括：
对待优化的地图进行预处理，以将所述待优化的地图上的地图点位置数据和关键帧相机位姿数据设定为图的顶点，以及，将各顶点之间的关系设定为所述图中各顶点之间的边，其中，所述关键帧相机位姿数据基于第一旋转矩阵表示；
基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第一反投影误差；
基于所述第一反投影误差、信息矩阵、鲁棒核参数和鲁棒核函数，获得第一鲁棒核误差；
基于所述第一旋转矩阵对所述图中各条边的两顶点进行处理，以获得所述图中各条边的两顶点之间的雅阁比矩阵；
基于所述第一反投影误差、所述信息矩阵和所述雅阁比矩阵，获得海塞伯矩阵和优化项；
对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值；
基于所述第一更新线性权值对所述海塞伯矩阵进行更新，以获得第二海塞伯格矩阵；
基于所述第二海塞伯格矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量，其中，所述位姿增量包括位姿平移增量和位姿旋转增量；
基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第一旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第一旋转矩阵，获得第二旋转矩阵，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据；
基于所述第二旋转矩阵对所述图中各条边对应的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第二反投影误差；
基于所述第二反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第二鲁棒核误差；以及
响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，输出所述第二旋转矩阵和更新后的各地图点位置数据，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。
在本申请一实施例中，所述SLAM后端优化算法，还包括：
响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，根据预设公式对所述海塞伯格矩阵的线性权值进行更新，以获得所述海塞伯矩阵的第二更新线性权值；
基于所述第二更新线性权值对所述第一更新的海塞伯格矩阵进行更新，以获得第二更新的海塞伯格矩阵；
基于所述第二更新的海塞伯格矩阵和所述优化项，获得所述待优化地图上的所有地图点位置增量以及关键帧位姿增量，其中，所述关键帧的位姿增量包括位姿平移增量和位姿旋转增量；
基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第二旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第二旋转矩阵，获得第三旋转矩阵，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据；
基于所述第三旋转矩阵对所述图中各条边对应的顶点进行反投影，并基于经过反投影处之后的各条边对应的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第三反投影误差；
基于所述第三反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第三鲁棒核误差；以及
响应于确定所述第三鲁棒核误差小于第二鲁棒核误差，输出所述第三旋转矩阵和更新后的各地图点位置数据，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据。
在本申请一实施例中，对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值的过程用公式表示为：
Figure BDA0001838168440000031
其中，Diagonal为计算矩阵的对角元，H为海塞伯格矩阵，Max为计算H矩阵最大对角元，Tau为0.00001，
Figure BDA0001838168440000041
为标量乘运算。
在本申请一实施例中，用以更新海塞伯格矩阵的线性权值的预设公式为：
Figure BDA0001838168440000042
其中，Nstep为λ的步进系数，λ为海塞伯格矩阵更新的线性权值，
Figure BDA0001838168440000043
为标量乘运算。
在本申请一实施例中，所述第一旋转矩阵为3x3的单位正交矩阵，其特征值为1。
根据本申请另一方面，还提供一种SLAM后端优化装置，其包括：
预处理单元，用于对待优化的地图进行预处理，以将所述待优化的地图上的地图点位置数据和关键帧相机位姿数据设定为图的顶点，以及，将各顶点之间的关系设定为所述图中各顶点之间的边，其中，所述关键帧相机位姿数据基于第一旋转矩阵表示；
反投影误差求解单元，用于基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第一反投影误差；
鲁棒核误差求解单元，用于基于所述第一反投影误差、信息矩阵、鲁棒核参数和鲁棒核函数，获得第一鲁棒核误差；
雅阁比矩阵求解单元，用于基于所述第一旋转矩阵对所述图中各条边的两顶点进行处理，以获得所述图中各条边的两顶点之间的雅阁比矩阵；
海塞伯矩阵更新单元，用于基于所述第一反投影误差、所述信息矩阵和所述雅阁比矩阵，获得海塞伯矩阵和优化项；对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值；以及，基于所述第一更新线性权值对所述海塞伯矩阵进行更新，以获得第二海塞伯格矩阵；
增量求解单元，用于基于所述第二海塞伯格矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量，其中，所述位姿增量包括位姿平移增量和位姿旋转增量；
更新单元，用于基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第一旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第一旋转矩阵，获得第二旋转矩阵，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据；
所述反投影误差求解单元，进一步用于基于所述第二旋转矩阵对所述图中各条边对应的顶点进行反投影处理，并基于经过反投影处之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第二反投影误差；
所述鲁棒核求解单元，进一步用于基于所述第二反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第二鲁棒核误差；以及
输出单元，用于响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，输出所述第二旋转矩阵和更新后的各地图点位置数据，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。
在申请一实施例中，所述海塞伯矩阵更新单元，进一步用于响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，根据预设公式对所述海塞伯格矩阵的线性权值进行更新，以获得所述海塞伯矩阵的第二更新线性权值；以及，基于所述第二更新线性权值对所述第一更新的海塞伯格矩阵进行更新，以获得第二更新的海塞伯格矩阵；
所述增量求解单元，进一步用于基于所述第二更新的海塞伯格矩阵和所述优化项，获得所述待优化地图上的所有地图点位置增量以及关键帧位姿增量，其中，所述关键帧的位姿增量包括位姿平移增量和位姿旋转增量；
所述更新单元，进一步用于基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第二旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第二旋转矩阵，获得第三旋转矩阵，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据；
所述反投影误差求解单元，进一步用于基于所述第三旋转矩阵对所述图中各条边对应的顶点进行反投影，并基于经过反投影处之后的各条边对应的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第三反投影误差；
所述鲁棒核误差求解单元，进一步用于基于所述第三反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第三鲁棒核误差；以及
所述输出单元，进一步用于响应于确定所述第三鲁棒核误差小于第二鲁棒核误差，输出所述第三旋转矩阵和更新后的各地图点位置数据，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据。
在本申请一实施例中，对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值的过程用公式表示为：
Figure BDA0001838168440000061
其中，Diagonal为计算矩阵的对角元，H为海塞伯格矩阵，Max为计算H矩阵最大对角元，Tau为0.00001，
Figure BDA0001838168440000062
为标量乘运算。
在本申请一实施例中，用以更新海塞伯格矩阵的线性权值的该预设公式为：
Figure BDA0001838168440000063
其中，Nstep为λ的步进系数，λ为海塞伯格矩阵更新的线性权值，
Figure BDA0001838168440000064
为标量乘运算。
在本申请一实施例中，所述第一旋转矩阵为3x3的单位正交矩阵，其特征值为1。
根据本申请的另一方面，还提供一种电子设备，其包括处理器和存储器，其中，在所述存储器中存储有计算机程序指令，所述计算机程序指令在被所述处理器运行时使得所述处理器执行如上所述的SLAM后端优化方法。
根据本申请的另一方面，还提供一种计算机可读存储介质，其上存储有计算机程序指令，当所述计算机程序指令被计算装置执行时，可操作来执行如上所述的SLAM后端优化方法。
通过对随后的描述和附图的理解，本申请进一步的目的和优势将得以充分体现。
本申请的这些和其它目的、特点和优势，通过下述的详细说明，附图和权利要求得以充分体现。
附图说明
图1图示了现有的SLAM后端优化方法的流程图。
图2图示了根据本申请较佳实施例的SLAM后端优化方法的流程图。
图3图示了根据本申请较佳实施例的SLAM后端优化方法的另一流程图。
图4图示了根据本申请较佳实施例的SLAM后端优化方法的又一流程图
图5图示了根据本申请较佳实施例的SLAM后端优化装置的框图。
图6图示了根据本申请实施例的电子设备的框图。
具体实施方式
以下描述用于揭露本申请以使本领域技术人员能够实现本申请。以下描述中的优选实施例只作为举例，本领域技术人员可以想到其他显而易见的变型。在以下描述中界定的本申请的基本原理可以应用于其他实施方案、变形方案、改进方案、等同方案以及没有背离本申请的精神和范围的其他技术方案。
本领域技术人员应理解的是，在本申请的揭露中，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系是基于附图所示的方位或位置关系，其仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此上述术语不能理解为对本申请的限制。
可以理解的是，术语“一”应理解为“至少一”或“一个或多个”，即在一个实施例中，一个元件的数量可以为一个，而在另外的实施例中，该元件的数量可以为多个，术语“一”不能理解为对数量的限制。
申请概述
如上所述，目前基于图的SLAM后端优化通常采用G2O框架(A General Frameworkfor Graph Optimization)。图1图示了现有的基于G2O框架进行SLAM后端优化方法的流程图。如图1所示，现有的SLAM后端优化方法主要包括步骤：首先，在进行预处理之前，将旋转矩阵转化为四元数以表示关键帧相机位姿；进而，对待优化的地图进行预处理，以将关键帧相机位姿和地图位置点配置为顶点，各顶点之间的关系配置为边；然后，基于四元数计算反投影误差；接着，求解鲁棒核误差；继而，基于四元数求解雅阁比矩阵；然后，求解海塞伯格矩阵和海塞伯格矩阵的更新线性权值，并基于该线性权值更新海塞伯格矩阵；进而，进行线性方程求解以获得地图点位置增量和关键帧相机位姿增量；接着，基于四元数、地图点位置增量和关键帧相机位姿增量对地图点位置和关键帧相机位姿进行更新；进而，求解更新的反投影误差和鲁棒核误差；接着，判定新获得的鲁棒核误差是否小于原先的鲁棒核误差，如果判定结果为小于，则结束优化返回四元数，并将四元数转化为旋转矩阵；如果判定结果为大于，则按照上述步骤进行循环直至满足条件为止。
然而，现有的SLAM后端优化方法，其计算量过于庞大。主要原因在于：在进行后端优化的计算过程中，关键帧相机位姿数据以四元数表示参与到各阶段的计算中。然而，在诸如预处理阶段、反投影误差计算阶段、雅阁比矩阵计算阶段和最终的数据返回阶段，该四元数必须先转化为旋转矩阵才能参与运算。这样一来一回的转化，增加了大量额外计算量，导致无法及时地完成对关键帧相机位姿和地图点位置的优化，进而导致关键帧相机位姿和地图点位置误差不断累积，影响最终的地图重建和即时定位的质量。
针对上述技术问题，本申请的基本构思是选择特定的旋转矩阵替代四元数关键帧相机位姿直接地参与到SLAM后端优化的各个计算阶段，通过这样的方式，减少SLAM后端优化的计算量，以及时地完成对关键帧相机位姿和地图点位置的优化，从而确保关键帧相机位姿和地图点位置误差不会累积，以保证最终的地图重建和即时定位的质量。
基于此，本申请提出了一种SLAM后端优化方法，其首先对待优化的地图进行预处理，以将所述待优化的地图上的地图点位置数据和关键帧相机位姿数据设定为图的顶点，以及，将各顶点之间的关系设定为所述图中各顶点之间的边，其中，所述关键帧相机位姿数据基于第一旋转矩阵表示；然后，基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第一反投影误差；接着，基于所述第一反投影误差、信息矩阵、鲁棒核参数和鲁棒核函数，获得第一鲁棒核误差；进而，基于所述第一旋转矩阵对所述图中各条边的两顶点进行处理，以获得所述图中各条边的两顶点之间的雅阁比矩阵；然后，基于所述第一反投影误差、所述信息矩阵和所述雅阁比矩阵，获得海塞伯矩阵和优化项；接着，对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值；然后，基于所述第一更新线性权值对所述海塞伯矩阵进行更新，以获得第二海塞伯格矩阵；进而，基于所述第二海塞伯格矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量，其中，所述位姿增量包括位姿平移增量和位姿旋转增量；然后，基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第一旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第一旋转矩阵，获得第二旋转矩阵，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据；然后，基于所述第二旋转矩阵对所述图中各条边对应的顶点进行反投影处理，并基于经过反投影处之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第二反投影误差；进而，基于所述第二反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第二鲁棒核误差；最终，响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，输出所述第二旋转矩阵和更新后的各地图点位置数据，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。这样，通过特定的旋转矩阵替代四元数表示关键帧相机位姿直接地参与到SLAM后端优化的各个计算阶段，通过这样的方式，减少SLAM后端优化的计算量，以及时地完成对关键帧相机位姿和地图点位置的优化，从而确保关键帧相机位姿和地图点位置误差不会累积，以保证最终的地图重建和即时定位的质量。
在介绍本申请的基本原理之后，下面将参考附图来具体介绍本申请的各种非限制性实施例。
示例性标定方法
图1图示了根据本申请较佳实施例的SLAM后端优化方法的流程图。如图1所示，根据本申请该较佳实施例的SLAM后端优化方法包括：S110，对待优化的地图进行预处理，以将所述待优化的地图上的地图点位置数据和关键帧相机位姿数据设定为图的顶点，以及，将各顶点之间的关系设定为所述图中各顶点之间的边，其中，所述关键帧相机位姿数据基于第一旋转矩阵表示；S120，基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第一反投影误差；S130，基于所述第一反投影误差、信息矩阵、鲁棒核参数和鲁棒核函数，获得第一鲁棒核误差；S140，基于所述第一旋转矩阵对所述图中各条边的两顶点进行处理，以获得所述图中各条边的两顶点之间的雅阁比矩阵；S150，基于所述第一反投影误差、所述信息矩阵和所述雅阁比矩阵，获得海塞伯矩阵和优化项；S160，对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值；S170，基于所述第一更新线性权值对所述海塞伯矩阵进行更新，以获得第二海塞伯格矩阵；S180，基于所述第二海塞伯格矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量，其中，所述位姿增量包括位姿平移增量和位姿旋转增量；S190，基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第一旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第一旋转矩阵，获得第二旋转矩阵，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据；S200,基于所述第二旋转矩阵对所述图中各条边对应的顶点进行反投影处理，并基于经过反投影处之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第二反投影误差；S210,基于所述第二反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第二鲁棒核误差；以及，S220,响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，输出所述第二旋转矩阵和更新后的各地图点位置数据，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。
在步骤S110中，对待优化的地图进行预处理，以将所述待优化的地图上的地图点位置数据和关键帧相机位姿数据设定为图的顶点，以及，将各顶点之间的关系设定为所述图中各顶点之间的边，其中，所述关键帧相机位姿数据基于第一旋转矩阵表示。这里，所述待优化的地图在SLAM(Simultaneous Localization and Mapping，同步定位和地图构建)前端处理中生成。本领域的技术人员应理解，在本申请中用以生成所述待优化的地图的SLAM前端处理算法并不为本申请所局限，其可选择任意一种现有的前端处理算法生成所述待优化的地图。
在本申请该实施例中，同样利用基于图的SLAM优化方法的思路进行SLAM后端优化。更具体地，首先将所述待优化地图上的地图点位置数据和关键帧相机位姿数据配置为图的顶点。同时，将各顶点之间的关系配置为所述图中各顶点之间的边，即，将所述地图点位置数据和地图点位置数据之间的关系，或者，地图点位置数据和关键帧相机位姿数据之间的关系，或者，关键帧相机位姿数据和关键帧相机位姿数据之间的关系，配置为所述图中的边。这样，通过对图中顶点和边的处理，便能够实现对所述待优化的地图进行优化。
特别地，在本申请该实施例中，所述关键帧相机位姿数据用旋转矩阵表示而非四元数表示。也就是说，相较于现有的SLAM后端优化算法，在进行预处理阶段之前，无需将旋转矩阵转化为四元数。因此，可降低计算量。特别地，所述第一旋转矩阵为特定的旋转矩阵，其与四元数具有不同的数据格式，但与四元数的物理意义相同(即，均表示三维坐标的旋转)。换言之，采用通过所述第一旋转矩阵替代四元数参与SLAM后端优化的技术方案，完全不影响SLAM后端优化的效果。更明确地，在本申请该实施例中，所述第一旋转矩阵为3x3的单位正交矩阵，其特征值为1。
在步骤S120中，基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第一反投影误差。换言之，在本申请该实施例中，所述反投影误差的计算过程基于所述第一旋转矩阵而非四元数，以进一步降低计算量。
更具体地，在本申请该实施例中，基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理的过程用公式可表示为：K*(R*X+T)，其中，K为相机内参，R、T为相机位姿的旋转矩阵和位移量，X为地图点位置坐标，以及，运算符*表示矩阵乘法。相应地，反投影误差求解过程用公式可表示为：Error＝Obs-K*(R*X+T)，其中，Obs为对应顶点的观测坐标，其为已知值。
这里，对比现有的基于四元数的反投影误差的计算过程，利用所述第一旋转矩阵替代四元数可直接地对三维坐标进行旋转(R*X)无需先将四元数转化旋转矩阵，从而可减少9次加运算、6次乘的计算量。换言之，在本申请该实施例中，每次反投影误差计算可节约9次加、6次乘的计算量，其中，总的反投影误差计算次数等于总的边数。
在步骤S130中，基于所述第一反投影误差、信息矩阵、鲁棒核参数和鲁棒核函数，获得第一鲁棒核误差。更具体地，求解第一鲁棒核误差的过程，用公式可表示为：
Figure BDA0001838168440000111
其中，errorT为反投影误差的转置，error为反投影误差，Ω为信息矩阵，f(x)为鲁棒核函数，δ为鲁棒核参数，
Figure BDA0001838168440000112
表示标量乘。
在步骤S140中，基于所述第一旋转矩阵对所述图中各条边的两顶点进行处理，以获得所述图中各条边的两顶点之间的雅阁比矩阵。换言之，在本申请该实施例中，所述雅阁比矩阵的计算过程基于所述第一旋转矩阵而非四元数，以进一步地降低计算量。
在现有基于四元数的雅阁比矩阵计算阶段中，其需先将四元数转化为旋转矩阵，方能对图中的顶点进行旋转处理。相对地，在本申请中，所述第一旋转矩阵可直接参与到雅阁比矩阵的计算过程中，从而可大幅地降低计算量。更明确地，每次雅阁比矩阵求解过程可减少的计算量包括：将四元数转化为旋转矩阵的计算量和利用四元数对图中的顶点进行旋转的计算量，分别为：9次加、12次的乘计算量，和9次加、6次乘的计算量，其中，总的雅阁比计算次数等于总的边数。
在步骤150中，基于所述第一反投影误差、所述信息矩阵和所述雅阁比矩阵，获得海塞伯矩阵和优化项。这里，计算海塞伯格矩阵的过程用公式表示为：H＝JT*Ω*J，其中，JT为雅阁比矩阵转置，Ω为信息矩阵，J为雅阁比矩阵。计算优化项的过程用公式可表示为：B＝error*Ω*J，其中，error为反投影误差，Ω为信息矩阵，J为雅阁比矩阵。
在步骤S160中，对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值。这里，获得所述海塞伯格矩阵的第一更新线性权值的过程用公式可表示为：
Figure BDA0001838168440000121
其中，Diagonal为计算矩阵的对角元，H为海塞伯格矩阵，Max为计算H矩阵最大对角元，Tau为0.00001，
Figure BDA0001838168440000122
为标量乘。值得一提的是，所述第一更新线性权值的作用在于对所述海塞伯格矩阵进行更新。
在步骤S170中，基于所述第一更新线性权值对所述海塞伯矩阵进行更新，以获得第二海塞伯格矩阵。这里，将所述海塞伯格矩阵更新为第二海塞伯格矩阵的过程，用公式可表示为：
Figure BDA0001838168440000123
其中，
Figure BDA0001838168440000124
表示第二海塞伯格矩阵，H为海塞伯格矩阵，λ为海塞伯格矩阵更新的线性权值，以及，I为单位对角阵。
在步骤S180中，基于所述第二海塞伯矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量，其中，所述位姿增量包括位姿平移增量和位姿旋转增量。更具体地，在本申请中，基于所述第二海塞伯矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量的过程，用公式可表示为：(H+λ*I)*Δx＝-B，其中，H为海塞伯格矩阵，λ为海塞伯格矩阵更新的第一线性权值，I为单位对角阵，B为优化项，Δx求解项，即，所述待优化地图上所有地图点位置增量和关键帧相机位姿增量。这里，本领域的技术人员应知晓，该过程同样被称为线性方程求解过程，其最终的求解结果为：所述待优化地图上所有地图点位置增量和关键帧相机位姿增量。
在步骤S190中，基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新。特别地，在本申请该实施例中，所述关键帧相机位姿更新过程，包括：基于所述第一旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第一旋转矩阵，获得第二旋转矩阵，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。
相较于现有的关键帧相机位姿更新过程，在本申请该实施例中，所述关键帧相机位姿的平移量更新采用所述第一旋转矩阵对位姿平移增量进行旋转得到更新的位姿平移量。所述关键帧相机位姿的旋转量的更新采用两次旋转矩阵的旋转，得到更新的位姿旋转量，其用公式可表示为：
Figure BDA0001838168440000131
其中，R为当前位姿的旋转矩阵，即所述第一旋转矩阵，ΔR为后端优化得到的旋转矩阵增量，
Figure BDA0001838168440000132
为更新增量后的旋转矩阵。也就是说，在本申请该实施例中，在利用关键帧相机位姿增量对关键帧相机位姿数据进行更新的过程中，不必做四元数到旋转矩阵的转化，并采用该转化的旋转矩阵对位姿平移增量做旋转，对旋转增量做二次旋转的累计，从而可大幅地降低运算量。
为了判定更新之后的所述地图点位置数据和所述关键帧相机位姿数据是否得到优化，需依据如上所述的方法获得更新的反投影误差和鲁棒核误差，并基于前后两次的鲁棒核误差之间的比较结果，选择终止优化返回结果或进行下一轮的优化。
更具体地，该判定过程包括步骤：S200，基于所述第二旋转矩阵对所述图中各条边对应的顶点进行反投影处理，并基于经过反投影处之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第二反投影误差；S210,基于所述第二反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第二鲁棒核误差；以及，S220,响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，输出所述第二旋转矩阵和更新后的各地图点位置数据，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。这里，求解所述第二反投影误差和所述第二鲁棒核误差的过程与求解所述反投影误差和所述鲁棒核误差的过程相一致，故在此不再赘述。
进一步地，当所述第二鲁棒核误差大于所述第一鲁棒核误差时，更新之后的所述地图位置点数据和所述关键帧相机位姿数据被设置进入新一轮的优化过程。相应地，该新一轮的优化过程包括步骤:S230，响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，根据预设公式对所述海塞伯格矩阵的线性权值进行更新，以获得所述海塞伯矩阵的第二更新线性权值；S240，基于所述第二更新线性权值对所述第一更新的海塞伯格矩阵进行更新，以获得第二更新的海塞伯格矩阵；S250，基于所述第二更新的海塞伯格矩阵和所述优化项，获得所述待优化地图上的所有地图点位置增量以及关键帧位姿增量，其中，所述关键帧的位姿增量包括位姿平移增量和位姿旋转增量；S260，基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第二旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第二旋转矩阵，获得第三旋转矩阵，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据；S270，基于所述第三旋转矩阵对所述图中各条边对应的顶点进行反投影，并基于经过反投影处之后的各条边对应的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第三反投影误差；S280，基于所述第三反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第三鲁棒核误差；以及S290，响应于确定所述第三鲁棒核误差小于第二鲁棒核误差，输出所述第三旋转矩阵和更新后的各地图点位置数据，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据。
应注意到，新一轮的优化过程与步骤S120-S220所阐释的内容相一致，除了所述步骤S230：根据预设公式对所述海塞伯格矩阵的线性权值进行更新，以获得所述海塞伯矩阵的第二更新线性权值。这里，在本申请该实施例中，用以更新海塞伯格矩阵的线性权值的该预设公式为：
Figure BDA0001838168440000141
其中，Nstep为λ的步进系数，λ为海塞伯格矩阵更新的线性权值，
Figure BDA0001838168440000142
为标量乘运算。
综上，通过特定的旋转矩阵替代四元数表示关键帧相机位姿，以减少SLAM后端优化的计算量的技术方案被阐明。
图4图示了根据本申请较佳实施例的SLAM后端优化方法的又一流程图，其为所述SLAM后端优化方法的简化流程图。如图4所示，根据本申请实施例的所述SLAM后端优化方法，包括：预处理配置顶点和边、基于旋转矩阵计算反投影误差、计算鲁棒核误差、基于旋转矩阵计算雅阁比矩阵、计算海塞伯格矩阵、计算海塞伯格矩阵更新线性权值、更新海塞伯格矩阵、线性方程求解、基于线性方程的求解结果和旋转矩阵更新地图位置点和关键帧相机位姿、基于旋转矩阵求解更新的反投影误差、计算更新的鲁棒核误差，以及，比较前后两次的鲁棒核误差，当比较结果为鲁棒核误差减小时，结束优化返回旋转矩阵；当比较结果为鲁棒核误差增大时、计算海塞伯格矩阵更新线性权值并进入新一轮的优化过程，直到满足鲁棒核误差减少的条件为止。相较于现有的技术方案，本申请所提供的SLAM后端优化方法中的计算量得到缩减。
更具体地，在预处理阶段中，使用第一旋转矩阵替换G2O的四元数作为相机位姿，一方面能够节约在后端优化的预处理前将旋转矩阵到四元数的计算；另一方面，可节约结果返回的四元数转化为旋转矩阵的计算。
在反投影误差计算阶段，当对图中顶点做三维坐标做旋转时，所述第一旋转矩阵替代四元数对三维坐标的旋转节约9次加、6次乘的计算量。也就是说，每次误差计算可节约9次加、6次乘的计算量，总的误差计算次数为总的边数量。
在雅阁比矩阵计算阶段，由于不必做四元数到旋转矩阵转换，每次雅阁比计算节约了9次加、12次的乘计算量；另外，当对图中顶点的三维坐标做旋转时，与反投影误差计算的旋转分析相同，每次雅阁比计算减少了9次加、6次乘的计算量，总的雅阁比计算次数为总的边数量。
在关键帧相机位姿更新阶段，同样不必做四元数到旋转矩阵的转换，这样，该部分的计算量同样得到缩减。
综上可以看出，本发明提供的所述SLAM后端优化方法通过特定的旋转矩阵替代四元数表示关键帧相机位姿数据，以能够有效地减少SLAM后端优化的计算量，从而能够及时地完成对关键帧相机位姿和地图点位置的优化，确保关键帧相机位姿和地图点位置误差不会累积，并保证最终的地图重建和即时定位的质量。
示意性SLAM后端优化装置
图5图示了根据本申请较佳实施例的SLAM后端优化装置的框图。
如图5所示，根据本申请该较佳实施例的SLAM后端优化装置500，其包括：预处理单元510，用于对待优化的地图进行预处理，以将所述待优化的地图上的地图点位置数据和关键帧相机位姿数据设定为图的顶点，以及，将各顶点之间的关系设定为所述图中各顶点之间的边，其中，所述关键帧相机位姿数据基于第一旋转矩阵表示；反投影误差求解单元520，用于基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第一反投影误差；鲁棒核误差求解单元530，用于基于所述第一反投影误差、信息矩阵、鲁棒核参数和鲁棒核函数，获得第一鲁棒核误差；雅阁比矩阵求解单元540，用于基于所述第一旋转矩阵对所述图中各条边的两顶点进行处理，以获得所述图中各条边的两顶点之间的雅阁比矩阵；海塞伯矩阵更新单元550，用于基于所述第一反投影误差、所述信息矩阵和所述雅阁比矩阵，获得海塞伯矩阵和优化项；对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值；以及，基于所述第一更新线性权值对所述海塞伯矩阵进行更新，以获得第二海塞伯矩阵；增量求解单元560，用于基于所述第二海塞伯矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量，其中，所述位姿增量包括位姿平移增量和位姿旋转增量；更新单元570，用于基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第一旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第一旋转矩阵，获得第二旋转矩阵，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据；所述反投影误差求解单元520，进一步用于基于所述第二旋转矩阵对所述图中各条边对应的顶点进行反投影处理，并基于经过反投影处之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第二反投影误差；所述鲁棒核求解单元530，进一步用于基于所述第二反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第二鲁棒核误差；以及，输出单元580，用于响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，输出所述第二旋转矩阵和更新后的各地图点位置数据，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。
在一个示例中，在上述SLAM后端优化装置500中，所述海塞伯矩阵更新单元550，进一步用于响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，根据预设公式对所述海塞伯格矩阵的线性权值进行更新，以获得所述海塞伯矩阵的第二更新线性权值；以及，基于所述第二更新线性权值对所述第一更新的海塞伯格矩阵进行更新，以获得第二更新的海塞伯格矩阵；所述增量求解单元560，进一步用于基于所述第二更新的海塞伯格矩阵和所述优化项，获得所述待优化地图上的所有地图点位置增量以及关键帧位姿增量，其中，所述关键帧的位姿增量包括位姿平移增量和位姿旋转增量；所述更新单元570，进一步用于基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第二旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第二旋转矩阵，获得第三旋转矩阵，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据；所述反投影误差求解单元520，进一步用于基于所述第三旋转矩阵对所述图中各条边对应的顶点进行反投影，并基于经过反投影处之后的各条边对应的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第三反投影误差；所述鲁棒核误差求解单元530，进一步用于基于所述第三反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第三鲁棒核误差；以及，所述输出单元580，进一步用于响应于确定所述第三鲁棒核误差小于第二鲁棒核误差，输出所述第三旋转矩阵和更新后的各地图点位置数据，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据。
在一个示例中，在上述SLAM后端优化装置500中，对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值的过程用公式表示为：
Figure BDA0001838168440000171
其中，Diagonal为计算矩阵的对角元，H为海塞伯格矩阵，Max为计算H矩阵最大对角元，Tau为0.00001，
Figure BDA0001838168440000172
为标量乘运算。
在一个示例中，在上述SLAM后端优化装置500中，用以更新海塞伯格矩阵的线性权值的该预设公式为：
Figure BDA0001838168440000173
其中，Nstep为λ的步进系数，λ为海塞伯格矩阵更新的线性权值，
Figure BDA0001838168440000174
为标量乘运算。
在一个示例中，在上述SLAM后端优化装置500中，所述第一旋转矩阵为3x3的单位正交矩阵，其特征值为1。
这里，本领域技术人员可以理解，上述SLAM后端优化装置500中的各个单元和模块的具体功能和操作已经在上面参考图1到图4描述的SLAM后端优化方法中详细介绍，并因此，将省略其重复描述。
如上所述，根据本申请实施例的SLAM后端优化装置可以实现在各种终端设备中，例如SLAM优化服务器。在一个示例中，根据本申请实施例的SLAM后端优化装置可以作为一个软件模块和/或硬件模块而集成到所述终端设备中。例如，该SLAM后端优化装置可以是该终端设备的操作系统中的一个软件模块，或者可以是针对于该终端设备所开发的一个应用程序；当然，该SLAM后端优化装置同样可以是该终端设备的众多硬件模块之一。
替换地，在另一示例中，该SLAM后端优化装置与该终端设备也可以是分立的终端设备，并且该SLAM后端优化装置可以通过有线和/或无线网络连接到该终端设备，并且按照约定的数据格式来传输交互信息。
示意性电子设备
下面，参考图6来描述根据本申请实施例的电子设备。
图6图示了根据本申请实施例的电子设备的框图。
如图6所示，电子设备10包括一个或多个处理器11和存储器12。
处理器11可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备10中的其他组件以执行期望的功能。
存储器12可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器11可以运行所述程序指令，以实现上文所述的本申请的各个实施例的SLAM后端优化方法以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如旋转矩阵等各种内容。
在一个示例中，电子设备10还可以包括：输入装置13和输出装置14，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。
例如，该输入装置13可以是例如键盘、鼠标等等。
该输出装置14可以向外部输出各种信息，包括旋转矩阵等。该输出设备14可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。
当然，为了简化，图6中仅示出了该电子设备10中与本申请有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备10还可以包括任何其他适当的组件。
示意性计算机程序产品
除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的SLAM后端优化方法中的步骤。
所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的SLAM后端优化方法中的步骤。
所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。
本申请中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。
还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。
提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。
本领域的技术人员应理解，上述描述及附图中所示的本申请的实施例只作为举例而并不限制本申请。本申请的目的已经完整并有效地实现。本申请的功能及结构原理已在实施例中展示和说明，在没有背离所述原理下，本申请的实施方式可以有任何变形或修改。

Claims (12)

1.一种SLAM后端优化方法，其特征在于，包括：
对待优化的地图进行预处理，以将所述待优化的地图上的地图点位置数据和关键帧相机位姿数据设定为图的顶点，以及，将各顶点之间的关系设定为所述图中各顶点之间的边，其中，所述关键帧相机位姿数据基于第一旋转矩阵表示；
基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第一反投影误差；
基于所述第一反投影误差、信息矩阵、鲁棒核参数和鲁棒核函数，获得第一鲁棒核误差；
基于所述第一旋转矩阵对所述图中各条边的两顶点进行处理，以获得所述图中各条边的两顶点之间的雅阁比矩阵；
基于所述第一反投影误差、所述信息矩阵和所述雅阁比矩阵，获得海塞伯矩阵和优化项；
对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值；
基于所述第一更新线性权值对所述海塞伯矩阵进行更新，以获得第二海塞伯格矩阵；
基于所述第二海塞伯格矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量，其中，所述位姿增量包括位姿平移增量和位姿旋转增量；
基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第一旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第一旋转矩阵，获得第二旋转矩阵，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据；
基于所述第二旋转矩阵对所述图中各条边对应的顶点进行反投影处理，并基于经过反投影处之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第二反投影误差；
基于所述第二反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第二鲁棒核误差；以及
响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，输出所述第二旋转矩阵和更新后的各地图点位置数据，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。
2.如权利要求1所述的SLAM后端优化算法，还包括：
响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，根据预设公式对所述海塞伯格矩阵的线性权值进行更新，以获得所述海塞伯矩阵的第二更新线性权值；
基于所述第二更新线性权值对所述第一更新的海塞伯格矩阵进行更新，以获得第二更新的海塞伯格矩阵；
基于所述第二更新的海塞伯格矩阵和所述优化项，获得所述待优化地图上的所有地图点位置增量以及关键帧位姿增量，其中，所述关键帧的位姿增量包括位姿平移增量和位姿旋转增量；
基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第二旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第二旋转矩阵，获得第三旋转矩阵，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据；
基于所述第三旋转矩阵对所述图中各条边对应的顶点进行反投影，并基于经过反投影处之后的各条边对应的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第三反投影误差；
基于所述第三反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第三鲁棒核误差；以及
响应于确定所述第三鲁棒核误差小于第二鲁棒核误差，输出所述第三旋转矩阵和更新后的各地图点位置数据，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据。
3.如权利要求1所述的SLAM后端优化方法，其中，对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值的过程用公式表示为：
Figure FDA0001838168430000031
其中，Diagonal为计算矩阵的对角元，H为海塞伯格矩阵，Max为计算H矩阵最大对角元，Tau为0.00001，
Figure FDA0001838168430000032
为标量乘运算。
4.如权利要求2所述的SLAM后端优化方法，其中，用以更新海塞伯格矩阵的线性权值的该预设公式为：
Figure FDA0001838168430000033
其中，Nstep为λ的步进系数，λ为海塞伯格矩阵更新的线性权值，
Figure FDA0001838168430000034
为标量乘运算。
5.如权利要求1-4任一所述的SLAM后端优化方法，其中，所述第一旋转矩阵为3x3的单位正交矩阵，其特征值为1。
6.一种SLAM后端优化装置，其特征在于，包括：
预处理单元，用于对待优化的地图进行预处理，以将所述待优化的地图上的地图点位置数据和关键帧相机位姿数据设定为图的顶点，以及，将各顶点之间的关系设定为所述图中各顶点之间的边，其中，所述关键帧相机位姿数据基于第一旋转矩阵表示；
反投影误差求解单元，用于基于所述第一旋转矩阵对所述图中各条边的顶点进行反投影处理，并基于经过反投影处理之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第一反投影误差；
鲁棒核误差求解单元，用于基于所述第一反投影误差、信息矩阵、鲁棒核参数和鲁棒核函数，获得第一鲁棒核误差；
雅阁比矩阵求解单元，用于基于所述第一旋转矩阵对所述图中各条边的两顶点进行处理，以获得所述图中各条边的两顶点之间的雅阁比矩阵；
海塞伯矩阵更新单元，用于基于所述第一反投影误差、所述信息矩阵和所述雅阁比矩阵，获得海塞伯矩阵和优化项；对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值；以及，基于所述第一更新线性权值对所述海塞伯矩阵进行更新，以获得第二海塞伯格矩阵；
增量求解单元，用于基于所述第二海塞伯格矩阵和所述优化项，获得所述待优化地图上所有地图点位置增量和关键帧相机位姿增量，其中，所述位姿增量包括位姿平移增量和位姿旋转增量；
更新单元，用于基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第一旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第一旋转矩阵，获得第二旋转矩阵，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据；
所述反投影误差求解单元，进一步用于基于所述第二旋转矩阵对所述图中各条边对应的顶点进行反投影处理，并基于经过反投影处之后的各条边的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第二反投影误差；
所述鲁棒核求解单元，进一步用于基于所述第二反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第二鲁棒核误差；以及
输出单元，用于响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，输出所述第二旋转矩阵和更新后的各地图点位置数据，其中，所述第二旋转矩阵表示更新的所述关键帧相机位姿数据。
7.如权利要求6所述的SLAM后端优化装置，其中，
所述海塞伯矩阵更新单元，进一步用于响应于确定所述第二鲁棒核误差小于第一鲁棒核误差，根据预设公式对所述海塞伯格矩阵的线性权值进行更新，以获得所述海塞伯矩阵的第二更新线性权值；以及，基于所述第二更新线性权值对所述第一更新的海塞伯格矩阵进行更新，以获得第二更新的海塞伯格矩阵；
所述增量求解单元，进一步用于基于所述第二更新的海塞伯格矩阵和所述优化项，获得所述待优化地图上的所有地图点位置增量以及关键帧位姿增量，其中，所述关键帧的位姿增量包括位姿平移增量和位姿旋转增量；
所述更新单元，进一步用于基于所述地图点位置增量和关键帧相机位姿增量，分别对所述待优化地图上的各地图点位置数据和关键帧相机位姿数据进行更新，其中，所述更新的过程，包括：基于所述第二旋转矩阵对所述位姿平移增量进行旋转；以及，基于旋转之后的所述位姿平移增量、所述位姿旋转增量和所述第二旋转矩阵，获得第三旋转矩阵，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据；
所述反投影误差求解单元，进一步用于基于所述第三旋转矩阵对所述图中各条边对应的顶点进行反投影，并基于经过反投影处之后的各条边对应的顶点坐标和对应顶点的观测坐标，获得所述图中各条边的第三反投影误差；
所述鲁棒核误差求解单元，进一步用于基于所述第三反投影误差、所述信息矩阵、所述鲁棒核函数和所述鲁棒核参数，获得第三鲁棒核误差；以及
所述输出单元，进一步用于响应于确定所述第三鲁棒核误差小于第二鲁棒核误差，输出所述第三旋转矩阵和更新后的各地图点位置数据，其中，所述第三旋转矩阵表示更新的所述关键帧相机位姿数据。
8.如权利要求7所述的SLAM后端优化装置，其中，对所述海塞伯矩阵进行处理，以获得所述海塞伯矩阵的第一更新线性权值的过程用公式表示为：
Figure FDA0001838168430000061
其中，Diagonal为计算矩阵的对角元，H为海塞伯格矩阵，Max为计算H矩阵最大对角元，Tau为0.00001，
Figure FDA0001838168430000062
为标量乘运算。
9.如权利要求8所述的SLAM后端优化装置，其中，用以更新海塞伯格矩阵的线性权值的该预设公式为：
Figure FDA0001838168430000063
其中，Nstep为λ的步进系数，λ为海塞伯格矩阵更新的线性权值，
Figure FDA0001838168430000071
为标量乘运算。
10.如权利要求6-9任一所述的SLAM后端优化装置，其中，所述第一旋转矩阵为3x3的单位正交矩阵，其特征值为1。
11.一种电子设备，其特征在于，包括：
处理器；和
存储器，在所述存储器中存储有计算机程序指令，所述计算机程序指令在被所述处理器运行时使得所述处理器执行如权利要求1-5中任一项所述的SLAM后端优化方法。
12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序指令，当所述计算机程序指令被计算装置执行时，可操作来执行如权利要求1-5中任一项所述的SLAM后端优化方法。
CN201811235928.2A 2018-10-24 2018-10-24 Slam后端优化方法、优化装置和电子设备 Active CN111091602B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811235928.2A CN111091602B (zh) 2018-10-24 2018-10-24 Slam后端优化方法、优化装置和电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811235928.2A CN111091602B (zh) 2018-10-24 2018-10-24 Slam后端优化方法、优化装置和电子设备

Publications (2)

Publication Number Publication Date
CN111091602A true CN111091602A (zh) 2020-05-01
CN111091602B CN111091602B (zh) 2023-03-28

Family

ID=70392651

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811235928.2A Active CN111091602B (zh) 2018-10-24 2018-10-24 Slam后端优化方法、优化装置和电子设备

Country Status (1)

Country Link
CN (1) CN111091602B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112130567A (zh) * 2020-09-22 2020-12-25 广州小鹏自动驾驶科技有限公司 一种数据处理方法和装置

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150262029A1 (en) * 2014-01-09 2015-09-17 Qualcomm Incorporated Sensor-based camera motion detection for unconstrained slam
CN107300917A (zh) * 2017-05-23 2017-10-27 北京理工大学 一种基于分层架构的视觉slam后端优化方法
US20180077534A1 (en) * 2016-09-13 2018-03-15 Google Inc. Systems and Methods for Graph-Based Localization and Mapping

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150262029A1 (en) * 2014-01-09 2015-09-17 Qualcomm Incorporated Sensor-based camera motion detection for unconstrained slam
US20180077534A1 (en) * 2016-09-13 2018-03-15 Google Inc. Systems and Methods for Graph-Based Localization and Mapping
CN107300917A (zh) * 2017-05-23 2017-10-27 北京理工大学 一种基于分层架构的视觉slam后端优化方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112130567A (zh) * 2020-09-22 2020-12-25 广州小鹏自动驾驶科技有限公司 一种数据处理方法和装置

Also Published As

Publication number Publication date
CN111091602B (zh) 2023-03-28

Similar Documents

Publication Publication Date Title
US20230023101A1 (en) Data processing method and device
CN109376631B (zh) 一种基于神经网络的回环检测方法及装置
CN109671020A (zh) 图像处理方法、装置、电子设备和计算机存储介质
CN110889446A (zh) 人脸图像识别模型训练及人脸图像识别方法和装置
CN113361527B (zh) 多目标对象的识别定位方法及装置、电子设备、存储介质
CN113449859A (zh) 一种数据处理方法及其装置
CN112651490B (zh) 人脸关键点检测模型的训练方法及设备、可读存储介质
CN111292334B (zh) 一种全景图像分割方法、装置及电子设备
WO2023164933A1 (zh) 一种建筑物建模方法以及相关装置
CN108304318B (zh) 设备兼容性的测试方法及终端设备
WO2021052010A1 (zh) 一种人脸朝向估计、网络训练方法、装置、电子设备及存储介质
CN111339724B (zh) 用于生成数据处理模型和版图的方法、设备和存储介质
CN109784415A (zh) 图像识别方法及装置、训练卷积神经网络的方法及装置
CN110956131A (zh) 单目标追踪方法、装置及系统
CN109409318B (zh) 统计模型的训练方法、统计方法、装置及存储介质
CN111160049A (zh) 文本翻译方法、装置、机器翻译系统和存储介质
CN111091602B (zh) Slam后端优化方法、优化装置和电子设备
WO2024061123A1 (zh) 一种图像处理方法及其相关设备
CN113205090A (zh) 图片矫正方法、装置、电子设备及计算机可读存储介质
CN113516029A (zh) 基于部分标注的图像人群计数方法、装置、介质及终端
CN112150529B (zh) 一种图像特征点的深度信息确定方法及装置
CN111723926A (zh) 用于确定图像视差的神经网络模型的训练方法和训练装置
CN112907501A (zh) 物体检测方法、装置及电子设备
US10467759B2 (en) Intelligent contouring of anatomy with structured user click points
WO2023236900A1 (zh) 一种项目推荐方法及其相关设备

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant