地图点坐标优化方法及装置、电子设备及存储介质

技术领域

本申请涉及地图技术领域，特别涉及一种地图构建的地图点坐标优化方法、地图点坐标优化装置、电子设备及存储介质。

背景技术

随着AR导航、自动驾驶等领域的应用在不断成熟，高精地图是实现这些应用的关键基础。而同时定位与地图构建(Simultaneous Location and Mapping，SLAM)作为底层的建图技术得到了越来越多的关注，可用于描述机器人在未知环境的未知地点中同步的进行地图构建和自身定位。

在SLAM算法问题中，可利用光束平差法(Bundle Adjustment，BA)对相机或激光的位姿和坐标进行联合优化，得到精度较高的地图点用于设备定位。传统的视觉SLAM的缺点在于BA优化的结果较大程度依赖于相机或激光的位姿估计精度和地图点的匹配精度。而由于相机或激光位姿估计结果不可避免的存在估计误差，导致BA优化后的地图点坐标与实际值会存在一定程度偏差。且对于快速运动或运动剧烈的应用场景，其初始值的误差较高，导致了最终地图点坐标的误差较高。

发明内容

有鉴于此，本申请的实施例提供了一种地图构建的地图点坐标优化方法、地图点坐标优化装置、电子设备及存储介质。

本申请提供了一种地图构建的地图点坐标优化方法，包括：

利用光束平差法对所述地图点坐标进行第一优化得到第一地图点坐标；

根据所述第一优化确定位姿误差与坐标误差的关系矩阵；

根据所述关系矩阵的矩阵迹建立优化模型以对所述第一地图点坐标进行第二优化得到第二地图点坐标。

本申请还提供了一种地图构建的地图点坐标优化方法，包括：

第一优化模块，用于利用光束平差法对所述地图点坐标进行第一优化得到第一地图点坐标；

矩阵模块，用于根据所述第一优化确定位姿误差与坐标误差的关系矩阵；

第二优化模块，用于根据所述关系矩阵的矩阵迹建立优化模型以对所述第一地图点坐标进行第二优化得到第二地图点坐标。

本申请还提供了一种电子设备，其特征在于，所述电子设备包括存储器和处理器，所述存储器中存储有计算机程序，所述计算机程序被所述处理器执行时，实现上述实施方式的地图点坐标优化方法。

本申请还提供了一种计算机程序的非易失性计算机可读存储介质，当所述计算机程序被一个或多个处理器执行时，实现上述实施方式的地图点坐标优化方法。

如此，本申请实施方式的地图点坐标优化方法、地图点坐标优化装置、电子设备及存储介质中，通过利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标，并根据第一优化确定位姿误差与坐标误差的关系矩阵，以及根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。相较于现有的BA优化仅通过对矩阵进行局部筛选以优化地图点坐标，本申请在BA优化的基础上通过对坐姿误差与坐标误差的关系矩阵迹建立优化模型，从全局对地图点坐标值进行优化，有效地校正了BA优化带来的误差，从而在一定程度上提升了地图点坐标的精度。同时，利用优化后的地图估计得到的相机或激光位姿的精度也更高。同时，对于相机或激光位姿及地图点坐标的初始值不精确的应用场景，如快速运动或运动剧烈等应用场景，容易导致相机或激光位姿及地图点坐标的间隔较大，其获取的初始值准确度较低，而现有的BA优化只是基于局部矩阵的筛选，对于初始值较差的场景，其最终的优化结果较差，导致地图精度较低。而本申请通过对全局的关系矩阵的迹进行最大化约束求解，可以在较大程度上校正初始值准确度较低的场景中BA优化带来的误差。另外，本申请的对所有利用BA优化得到的地图的系统均可适用，如单目视觉SLAM、双目视觉SLAM、RGB-D或者激光SLAM系统中都具有一定的适用性，其适应性较广。

附图说明

本申请的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解。

图1是本申请某些实施方式的地图点坐标优化方法流程示意图；

图2是本申请某些实施方式的地图点坐标优化装置模块图；

图3是本申请某些实施方式的地图点坐标优化方法流程示意图；

图4是本申请某些实施方式的地图点坐标优化方法流程示意图；

图5是本申请某些实施方式的地图点坐标优化方法流程示意图；

图6是本申请某些实施方式的地图点坐标优化方法流程示意图；

图7是本申请某些实施方式的地图点坐标优化方法流程示意图；

图8是本申请某些实施方式的地图点坐标优化装置模块图；

图9是本申请某些实施方式的地图点坐标优化方法流程示意图；

图10是本申请某些实施方式的地图点坐标优化装置模块图；

图11是本申请某些实施方式的地图点坐标优化方法实验结果图；

图12是本申请某些实施方式的地图点坐标优化方法实验结果图；

图13是本申请某些实施方式的地图点坐标优化方法实验结果图。

具体实施方法

下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。

请参阅图1，本申请提供了一种用于地图构建的地图点坐标优化方法，其特征在于，包括：

S10：利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标；

S20：根据第一优化确定位姿误差与坐标误差的关系矩阵；

S30：根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。

相应地，请参阅图2，本申请实施方式还提供了一种地图点坐标优化装置100，本申请实施方式的地图点坐标优化方法可以由地图点坐标优化装置100实现。地图点坐标优化装置100包括第一优化模块110、矩阵模块120及第二优化模块130。S10可以由第一优化模块110实现，S20可以由矩阵模块120实现，S30可以由第二优化模块130实现。或者说，第一优化模块110用于利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标。矩阵模块120用于根据第一优化确定相机的位姿误差与坐标误差的关系矩阵。第二优化模块130用于根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。

本申请实施方式还提供了一种电子设备。电子设备包括存储器和处理器。存储器中存储有计算机程序，处理器用于利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标，并根据第一优化确定相机的位姿误差与坐标误差的关系矩阵，以及根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。

具体地，在步骤S10中，利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标。

可以理解，在光束平差法(Bundle Adjustment，BA)BA优化前还包括前端得到粗略的地图点坐标，得到粗略的地图点坐标的方法为SLAM的通常前端方法，此处不展开说明。

当得到粗略的地图点坐标之后，对地图点坐标进行BA优化。可以理解，BA优化为视觉SLAM中的后端优化方法，通过优化模型进行最小化重投影误差。BA优化可以优化相机或激光的位姿，还可以优化特征点的空间位置即地图点坐标。

进一步地，通过BA优化算法求解得到第一优化后的地图点坐标，即就是第一地图点坐标。对于BA优化算法，可利用高斯牛顿求解法、牛顿法、梯度下降法等算法对BA优化的优化模型进行求解。

请参阅图3，优选地，在某些实施方法中，步骤S10包括：

S11：根据位姿和地图点坐标确定光束平差法的最小二乘问题，其中位姿包括相机位姿或激光位姿；

S12：利用高斯-牛顿法对最小二乘问题进行求解以得到第一地图点坐标。

在某些实施方法中，S11和S12可以由地图点坐标优化装置100的第一优化模块110实现。或者说，第一优化模块110用于根据位姿和地图点坐标确定光束平差法的最小二乘问题，其中位姿包括相机位姿或激光位姿，并利用高斯-牛顿法对最小二乘问题进行求解以得到第一地图点坐标。

在某些实施方法中，处理器用于根据位姿和地图点坐标确定光束平差法的最小二乘问题，其中位姿包括相机位姿或激光位姿，并利用高斯-牛顿法对最小二乘问题进行求解以得到第一地图点坐标。

需要说明的是，本申请可用于单目视觉SLAM、双目视觉SLAM、RGB-D或者激光SLAM系统，以下为单目视觉SLAM为例展开实施例。

具体地，在视觉SLAM中，由于两张图片有很多对特征匹配点，将所有特征点误差求和，取平方，再乘上1/2，可构建一个非线性最小二乘问题。再将其中的位姿、地图点坐标作为优化的对象，最小化重投影误差，即可得到一个优化后的结果。如可采用如下BA优化公式：

其中，h(x,p

进一步地，可利用高斯-牛顿法对最小二乘问题进行求解以得到第一地图点坐标。高斯-牛顿法是非线性最小二乘的一种优化算法，采用逐步迭代的方法，把非线性问题变成一步一步迭代的线性问题，从而达到极值收敛。

在步骤S20中，当利用BA优化进行第一优化时，可确定位姿误差与坐标误差的关系矩阵。其中，位姿误差包括但不限于相机的位姿误差。关系矩阵可为H稀疏矩阵。例如利用高斯-牛顿法对BA优化进行计算求解得到相机位姿误差E(ε

进一步地，在步骤S30中，根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。

具体地，矩阵迹

请参阅图4，优选地，在某些实施方法中，步骤S30包括：

S31：建立对矩阵迹的值最大化的优化模型；

S32：对优化模型求解以得到第二地图点坐标。

在某些实施方法中，S31和S32可以由地图点坐标优化装置100的第二优化模块130实现。或者说，第二优化模块130用于建立对矩阵迹的值最大化的优化模型，对优化模型求解以得到第二地图点坐标。

在某些实施方法中，处理器用于建立对矩阵迹的值最大化的优化模型，对优化模型求解以得到第二地图点坐标。

具体地，对步骤S20中所得到的关系矩阵即H稀疏矩阵进行矩阵迹

如此，根据机器学习理论中信息选择理论可知，矩阵的迹在一定程度上表征了地图点的质量，迹的值越大则地图点的质量越高。本实施方法通过矩阵迹值进行最大化求解从而再次对地图点坐标进行优化，在一定程度上提高了地图点的质量。相较于现有的BA优化仅通过对矩阵进行局部筛选以优化地图点坐标，本申请在BA优化的基础上从全局对地图点坐标值进行优化，有效地校正了BA优化带来的误差，从而在一定程度上提升了地图点坐标的精度。

请参阅图5，优选地，在某些实施方法中，优化模型包括：

s.t.

|h(x,p

|h(x,p

…

|h(x,p

其中，

相应地，步骤S32包括：

S321：通过对第一地图点坐标进行调整以使得矩阵迹的值最大化。

具体地，以矩阵迹

|h(x,p

|h(x,p

…

|h(x,p

进一步地，通过对第一地图坐标点p

需要说明的是，上述以视觉SLAM为例对以地图点坐标为约束条件进行说明，对于其他SLAM系统，可相应地调整优化模型。

如此，通过调整地图点的坐标与相机坐标点的距离在阈值范围内以使得H矩阵的矩阵迹的值最大。相较于以其它参数如位姿为约束条件，其计算量更小，同时最终优化后的地图点坐标更准确。

请参阅图6，在某些实施方法中，地图点坐标包括里程计地图点坐标和回环地图点坐标，步骤S10还包括：

S13：当地图点坐标为里程计地图点坐标时，利用光束平差法对里程计地图点坐标进行第一优化得到第一地图点坐标；

S14：当地图点坐标为回环地图点坐标时，利用光束平差法对回环地图点坐标进行第一优化得到第一地图点坐标。

在某些实施方法中，S13和S14可以由地图点坐标优化装置100的第一优化模块110实现。或者说，第一优化模块110用于当地图点坐标为里程计地图点坐标时，利用光束平差法对里程计地图点坐标进行第一优化得到第一地图点坐标，当地图点坐标为回环地图点坐标时，利用光束平差法对回环地图点坐标进行第一优化得到第一地图点坐标。

在某些实施方法中，处理器用于当地图点坐标为里程计地图点坐标时，利用光束平差法对里程计地图点坐标进行第一优化得到第一地图点坐标，当地图点坐标为回环地图点坐标时，利用光束平差法对回环地图点坐标进行第一优化得到第一地图点坐标。

可以理解，视觉SLAM分为前端和后端，而前端包括视觉里程计和回环检测。其中，视觉里程计研究相邻图像帧之间的变换关系以完成实时的位姿跟踪从而得到运动关系。而回环检测，又称为闭环检测，主要是利用图像之间的相似性来判断是否到达先前的位置，以此来消除累计误差，从而得到全局一致性轨迹和地图。故当优化的地图点坐标为里程计地图点坐标时，利用光束平差法对里程计地图点坐标进行第一优化得到第一地图点坐标，当优化的地图点坐标为回环地图点坐标时，利用光束平差法对回环地图点坐标进行第一优化得到第一地图点坐标。即就是，本申请可同时用于里程计线程与回环检测的后端优化。

请参阅图7，在某些实施方法中，步骤S10之前还包括：

S01：利用摄像头采集环境视频；

S02：对环境视频进行特征提取以得到帧间图像点的匹配关系；

S03：利用匹配关系确定地图点坐标。

相应地，请参阅图8，本申请实施方式的地图点坐标优化装置100还包括采集模块140、提取模块150及确定模块160。S01可以由采集模块140实现，S02可以由提取模块150实现，S03可以由确定模块160实现。或者说，采集模块140用于利用摄像头采集环境视频。提取模块150用于对环境视频进行特征提取以得到帧间图像点的匹配关系。确定模块160用于利用匹配关系确定地图点坐标。

在某些实施方法中，处理器用于利用摄像头采集环境视频，对环境视频进行特征提取以得到帧间图像点的匹配关系，及利用匹配关系确定地图点坐标。

具体地，首先需要利用摄像头在需要建图的环境中不断采集视频，可以理解的是，为保证能够建出高质量的地图，同一地点可重复多次拍摄。进一步地，对拍摄的视频进行特征提取或光流计算，从而得到帧间图像点的匹配关系。特征点匹配可采用光流跟踪得到关键点的匹配。同时为了排除误跟踪，可以采用一致性检测。而如果两帧之间的运动量和外观变化较大，则需要计算两帧之间的特征点和描述子，比较描述子之间的距离。由于计算量关系，也可采用恒速模型在预期区域中搜索潜在的对应关系。而如果是双目匹配或者深度滤波器中计算每个像素的深度，则可采用极线搜索和归一化互相关或绝对误差和找到匹配点。当确定特征匹配后，利用该匹配关系可以计算出基本矩阵F或单应矩阵H，然后对F或H进行奇异值分解得到两帧间相机的位姿，根据该位姿和图像点的对应关系利用三角化方法即可得到粗略的地图点坐标。

进一步地，利用本申请的上述任一种优化方法可对粗略的地图点坐标进行优化从而得到第二地图点坐标。

请参阅图9，在某些实施方法中，地图点坐标优化方法还包括：

S40：根据第二地图点坐标构建地图。

相应地，请参阅图10，本申请实施方式的地图点坐标优化装置100还包括构图模块170。S40可以由构图模块170实现。或者说，构图模块170用于根据第二地图点坐标构建地图。

在某些实施方法中，处理器用于根据第二地图点坐标构建地图。

具体地，当前端以及后端的第二优化完成后得到第二地图点坐标，则视觉SLAM可根据优化后的第二地图点坐标构建地图。可以理解的是，视觉SLAM中的构建地图包括路标地图、拓扑地图、度量地图和混合地图等。可通过三角测量或深度估计，将平面图像中的信息转换为空间3D路标点等。

如此，在视觉SLAM中在BA优化的基础上通过第二次优化，可以有效地校正BA优化带来的误差，在一定程度上提升了地图点坐标的精度。

请参阅图11-13，在一些实例中，将本申请的方案在ORBSLAM系统中进行了实际应用，并同时与没有实施本申请的ORBSLAM优化进行了对比验证。图11-13为三个实施例的EuroC公开数据集上的测试结果，每一个为不同的应用场景。其中纵坐标为绝对位姿误差或者说绝对轨迹误差APE，横坐标左边方框our表示应用本申请的ORBSLAM优化得到的相机或激光位姿误差，横坐标右边ORB-SLAM表示传统ORBSLAM系统相机或激光位姿估计误差。

从图中可以看出应用本申请的ORBSLAM优化得到的相机或激光位姿误差估计较大程度低于ORBSLAM。

综上所述，在本申请实施方式的地图点坐标优化方法、地图点坐标优化装置、电子设备及存储介质中，通过利用光束平差法对地图点坐标进行第一优化得到第一地图点坐标，并根据第一优化确定位姿误差与坐标误差的关系矩阵，以及根据关系矩阵的矩阵迹建立优化模型以对第一地图点坐标进行第二优化得到第二地图点坐标。至少具有以下有益效果：

一、在BA优化的基础上对矩阵迹值进行最大化求解从而再次对地图点坐标进行优化，在一定程度上提高了地图点的质量。

二、相较于现有的BA优化仅通过对矩阵进行局部筛选以优化地图点坐标，本申请在BA优化的基础上通过对坐姿误差与坐标误差的关系矩阵迹建立优化模型，从全局对地图点坐标值进行优化，有效地校正了BA优化带来的误差，从而在一定程度上提升了地图点坐标的精度。

三、利用优化后的地图估计得到的相机或激光位姿的精度也更高。

四、本申请的对所有利用BA优化得到的地图的系统均可适用，如单目视觉SLAM、双目视觉SLAM、RGB-D或者激光SLAM系统中都具有一定的适用性，其适应性较广。

五、对于相机或激光位姿及地图点坐标的初始值不精确的应用场景，如快速运动或运动剧烈等应用场景，容易导致相机或激光位姿及地图点坐标的间隔较大，其获取的初始值准确度较低，而现有的BA优化只是基于局部矩阵的筛选，对于初始值较差的场景，其最终的优化结果较差，导致地图精度较低。而本申请通过对全局的关系矩阵的迹进行最大化约束求解，可以在较大程度上校正初始值准确度较低的场景中BA优化带来的误差。

本申请实施方式还提供了一种计算机可读存储介质。一个或多个存储有计算机程序的非易失性计算机可读存储介质，当计算机程序被一个或多个处理器执行时，实现上述任一实施方式的地图点坐标优化方法。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的软件来完成。程序可存储于一非易失性计算机可读存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)等。

以上实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。