CN116630442B - 一种视觉slam位姿估计精度评估方法及装置 - Google Patents

Abstract

本发明实施例公开了一种视觉SLAM位姿估计精度评估方法及装置，其中，该方法包括：计算获取当前图像帧包含的特征点在视觉SLAM地图中所对应的地图点的第一深度值与第二深度值，并计算各地图点的第一深度值与第二深度值之间的差值，根据各地图点的深度差值以及权重评估视觉SLAM位姿估计的精度。本技术方案不需要依赖外部更高精度传感器获取的位姿信息，仅根据自身监测到的内部数据，即地图点信息就可以评估视觉SLAM位姿估计的精度，可以有效提高视觉SLAM系统精度评估的实用性和可靠性。

Description

一种视觉SLAM位姿估计精度评估方法及装置

技术领域

本发明涉及图像处理技术领域，尤其涉及一种视觉SLAM位姿估计精度评估方法及装置。

背景技术

视觉SLAM（Simultaneous Localization and Mapping，即同时定位与地图构建）是一种利用相机图像数据实现机器人定位和环境地图构建的技术。它在许多领域，如自动驾驶、无人机、增强现实和机器人导航中具有广泛应用。

在视觉SLAM地图系统中，精度评估是评估定位和地图构建结果的准确性和可靠性的关键问题，现有技术中以激光雷达SLAM融合厘米级RTK定位结果作为参考真值坐标，将视觉里程计或视觉SLAM地图定位结果与参考真值坐标进行比较，评估视觉定位的精度并绘制显示定位轨迹及精度评估结果。但是这种方法存在着限制，其参考真值数据依赖RTK或者激光雷达提供，然而高精度的参考数据可能不够准确或无法获得，从而影响评估的可靠性。

传统视觉SLAM位姿估计精度评估方法依赖于外部更高精度传感器或已知地图信息，限制了视觉SLAM位姿估计精度评估的实用性。因此，需要一种基于内部数据的精度评估方法，提高视觉SLAM位姿估计精度评估的实用性和可靠性。

发明内容

本发明的主要目的在于提供一种视觉SLAM位姿估计精度评估方法、装置、计算机设备及存储介质，可以解决现有技术中的视觉SLAM位姿估计精度评估方法具有较大局限性的问题。

为实现上述目的，本发明第一方面提供一种视觉SLAM位姿估计精度评估方法，所述方法包括：

获取当前图像帧包含的特征点在视觉SLAM地图中所对应的地图点，得到地图点集，根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第i个地图点对应的第一深度值；根据由所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第i个地图点对应的第二深度值；其中，i的取值从1至N，N为地图点集中的地图点总数；

计算所述地图点集中第i个地图点的第一深度值和第二深度值的差值，得到所述第i个地图点对应的深度差值；

根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉SLAM位姿估计精度。

结合第一方面，在一种可能的实现方式中，上述根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，包括：若所述第i个地图点对应的特征点只存在于所述当前图像帧内，则确定所述第i个地图点对应的深度差值的权重为第一权重值；若所述第i个地图点对应的特征点存在于所述当前图像帧以及历史图像帧中的关键帧内，并且第i个地图点不为目标地图点，则确定所述第i个地图点对应的深度差值的权重为第二权重值；其中，所述目标地图点为根据所述当前图像帧的位姿以及所述当前图像帧中特征点的三维坐标更新过地图点信息的地图点，所述地图点信息至少包括地图点的坐标信息；若所述第i个地图点对应的特征点存在于所述当前图像帧以及历史图像帧中的关键帧内，并且第i个地图点为目标地图点，则确定所述第i个地图点对应的深度差值的权重为第三权重值；其中，所述第三权重值大于所述第二权重值，所述第二权重值大于所述第一权重值。

结合第一方面，在一种可能的实现方式中，上述根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉SLAM位姿估计精度，包括：根据所述地图点集中所有地图点对应的权重以及深度差值，计算方差累加值；计算所述地图点集中所有地图点对应的权重之和，得到目标值，根据所述目标值与所述方差累加值计算所述视觉SLAM位姿估计精度。

结合第一方面，在一种可能的实现方式中，上述根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第i个地图点对应的第一深度值，包括：若所述当前图像帧为双目相机所拍摄，则所述当前图像帧包括当前左图像帧和当前右图像帧，获取所述第i个地图点对应的特征点分别在当前左图像帧和当前右图像帧中像素坐标系下的像素坐标，对应得到第一像素坐标和第二像素坐标，根据所述第一像素坐标和所述第二像素坐标计算视差值，得到第一视差值；根据所述第一视差值、双目相机的焦距以及双目相机的基线长度，计算所述第i个地图点对应的特征点的深度值，得到第一深度值。

结合第一方面，在一种可能的实现方式中，上述根据所述第一像素坐标和所述第二像素坐标计算视差值，包括：计算所述第一像素坐标和所述第二像素坐标的水平坐标之差，得到第一视差值。

结合第一方面，在一种可能的实现方式中，上述根据所述第一视差值、双目相机的焦距以及双目相机的基线长度，计算所述第i个地图点对应的特征点的深度值，得到第一深度值，包括：计算双目相机的焦距以及双目相机的基线长度的乘积；以所述乘积为被除数，计算所述乘积与所述第一视差值的商，得到第一深度值。

结合第一方面，在一种可能的实现方式中，上述目标坐标为第一目标坐标或第二目标坐标，所述根据由所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第i个地图点对应的第二深度值，包括：若所述当前图像帧为双目相机所拍摄，则所述当前图像帧包括当前左图像帧和当前右图像帧，根据所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到目标坐标的计算公式为：

其中，表示第i个地图点在世界坐标系下的坐标，/>为第i个地图点的第一深度值，K为双目相机内参数矩阵，R为旋转矩阵，t为当前左图像帧或者当前右图像帧在世界坐标系中的坐标，若t为当前左图像帧在世界坐标系中的坐标，则（u，v）为第一目标坐标，若t为当前右图像帧在世界坐标系中的坐标，则（u，v）为第二目标坐标；若所述（u，v）为第一目标坐标，则根据所述第一目标坐标通过特征点匹配所述第二目标坐标，或者若所述（u，v）为第二目标坐标，则根据所述第二目标坐标通过特征点匹配所述第一目标坐标；若匹配成功，则计算第一目标坐标与所述第二目标坐标的水平坐标差值，得到第二视差值，根据所述第二视差值、双目相机的焦距以及双目相机的基线长度，计算所述第i个地图点对应的特征点的深度值，得到所述第i个地图点对应的第二深度值；若匹配失败，则将所述第i个地图点从地图点集合中删除。

结合第一方面，在一种可能的实现方式中，上述根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第i个地图点对应的第一深度值，包括：若所述当前图像帧为RGB-D相机所拍摄，则根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，从所述当前图像帧对应的深度图像中确定所述像素坐标对应的深度值，得到第一深度值。

结合第一方面，在一种可能的实现方式中，上述根据由所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第i个地图点对应的第二深度值，包括：若所述当前图像帧为RGB-D相机所拍摄，根据所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵计算目标坐标；从所述当前图像帧对应的深度图像中确定所述目标坐标对应的深度值，得到第二深度值。

为实现上述目的，本发明第二方面提供一种视觉SLAM位姿估计精度评估装置，所述装置包括：

深度值计算模块：用于获取当前图像帧包含的特征点在视觉SLAM地图中所对应的地图点，得到地图点集，根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第i个地图点对应的第一深度值；根据由所述第i个地图点对应的特征点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第i个地图点对应的第二深度值；其中，i的取值从1至N，N为地图点集中的地图点总数；

深度差值计算模块：用于计算所述地图点集中第i个地图点的第一深度值和第二深度值的差值，得到所述第i个地图点对应的深度差值；

精度计算模块：用于根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉SLAM位姿估计精度。

为实现上述目的，本发明第三方面提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行以下步骤：

获取当前图像帧包含的特征点在视觉SLAM地图中所对应的地图点，得到地图点集，根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第i个地图点对应的第一深度值；根据由所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第i个地图点对应的第二深度值；其中，i的取值从1至N，N为地图点集中的地图点总数；

计算所述地图点集中第i个地图点的第一深度值和第二深度值的差值，得到所述第i个地图点对应的深度差值；

根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉SLAM位姿估计精度。

为实现上述目的，本发明第四方面提供一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行以下步骤：

获取当前图像帧包含的特征点在视觉SLAM地图中所对应的地图点，得到地图点集，根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第i个地图点对应的第一深度值；根据由所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第i个地图点对应的第二深度值；其中，i的取值从1至N，N为地图点集中的地图点总数；

计算所述地图点集中第i个地图点的第一深度值和第二深度值的差值，得到所述第i个地图点对应的深度差值；

根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉SLAM位姿估计精度。

采用本发明实施例，具有如下有益效果：

本发明提供一种视觉SLAM位姿估计精度评估方法，通过计算在视觉SLAM地图中当前图像帧包含的特征点对应的地图点的第一深度值和第二深度值，以及计算各个地图点对应的第一深度值与第二深度值的差值，得到各个地图点对应的深度差值，其中，根据第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定第i个地图点对应的第一深度值，根据由第i个地图点对应的特征点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定第i个地图点对应的第二深度值，最后，根据预设规则确定各个地图点对应的深度差值的权重，根据各个地图点对应的权重以及深度差值计算视觉SLAM位姿估计精度。在本技术方案中，通过视觉SLAM地图中当前图像帧包含的特征点对应的地图点的坐标来确定第一深度值与第二深度值，并根据第一深度值与第二深度值之间的差值以及权重评估视觉SLAM位姿估计精度，不需要

依赖外部更高精度传感器获取的地图信息，仅根据自身监测到的内部数据，即地图点信息就可以评估视觉SLAM位姿估计精度，可以有效提高视觉SLAM地图系统精度评估的实用性和可靠性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

其中：

图1为本发明实施例中一种视觉SLAM位姿估计精度评估方法的流程图；

图2为本发明实施例中一种视觉SLAM位姿估计精度评估装置的结构框图；

图3为本发明实施例中计算机设备的结构框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

视觉 SLAM( Simultaneous Localization and Mapping)是一种利用相机图像进行同时定位和地图构建的技术，它是机器人导航和自主驾驶汽车等领域中的重要技术之一，视觉SLAM的原理是通过相机捕捉环境中的图像，从中提取特征点，并利用这些特征点进行地图构建和定位，主要根据特征点得到地图点的位姿定位地图点，以构建视觉 SLAM地图，位姿可以理解为是描述地图点的位置和姿态，视觉SLAM地图中包含了多个根据图像帧中提取的特征点得到的地图点以及地图点的信息，地图点的信息至少包括地图点坐标。其中，在图像处理中，特征点指的是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点(即两个边缘的交点)，如角点、边缘点或尺度不变特征点（如：SIFT、ORB等），图像特征点在基于特征点的图像匹配算法中有着十分重要的作用，图像特征点能够反映图像本质特征，能够标识图像中目标物体。

本发明实施例提供了一种视觉SLAM位姿估计精度评估方法，该方法用于估计视觉SLAM位姿精度，在本实施例中，视觉SLAM位姿估计精度可以理解为视觉SLAM地图中的地图点的位姿估计精度，因此，相当于估计视觉SLAM地图的精度，参照图1，图1为本发明实施例提供的一种视觉SLAM位姿估计精度评估方法的流程示意图，如图1所示，该方法具体步骤如下：

步骤S101、获取当前图像帧包含的特征点在视觉SLAM地图中所对应的地图点，得到地图点集，根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第i个地图点对应的第一深度值；根据由所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第i个地图点对应的第二深度值。

其中，i的取值从1至N，N为地图点集中的地图点总数。

步骤S102、计算所述地图点集中第i个地图点的第一深度值和第二深度值的差值，得到所述第i个地图点对应的深度差值；

步骤S103、根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉SLAM位姿估计精度。

获取在视觉SLAM地图中的对象地图点，得到地图点集合，其中，该对象地图点为当前图像帧包含的特征点所对应的地图点，当前图像帧为相机最新拍摄的图像，在本实施例中，为了减少计算量以及提高精度估计速度，可以选取n个在视觉SLAM地图中均匀分布的对象地图点，比如，可以预先设置多个位置范围，每个位置范围间隔相等，选取在预设的位置范围的对象地图点，得到地图点集合，

选取n个在视觉SLAM地图中均匀分布的对象地图点，可以避免由于局部密集的地图点精确度不高，导致最终计算的视觉SLAM位姿估计精度不准确。

在一种可能的实现方式中，可以根据一定的筛选规则，对当前图像帧中的特征点进行进行筛选，去除低质量的特征点，将筛选后剩下的特征点映射至视觉SLAM地图中，得到对应的地图点，可以提高地图点集合中地图点的质量，进而提高视觉SLAM位姿估计精度评估结果。其中，对于双目相机拍摄的当前图像帧，则当前图像帧包括左当前图像帧和右当前图像帧，可以通过特征点匹配方法，根据左当前图像帧的特征点匹配右当前图像帧中同一个特征点，得到左、右当前图像帧中特征点之间的对应关系，若匹配失败，则可以去除该特征点。

获取地图点集合中各个地图点在当前图像帧中所对应的特征点的像素坐标，该像素坐标为像素坐标系下的二维坐标，根据第i个地图点所对应的特征点的像素坐标，确定第i个地图点对应的第一深度值。

具体地，在一种可能的实现方式中，若拍摄当前图像帧的相机为双目相机，双目相机是采用双目技术拍摄图像的一种相机，它是利用两台相机，每台相机分别拍摄不同的图像，然后将这两台相机的图像进行对比，从而计算出物体的距离或深度信息，即当当前图像帧为双目相机所拍摄，则当前图像帧包括当前左图像帧和当前右图像帧，则根据第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定第i个地图点对应的第一深度值的方法如步骤S201-步骤S202所示：

步骤S201、获取所述第i个地图点对应的特征点分别在当前左图像帧和当前右图像帧中像素坐标系下的像素坐标，对应得到第一像素坐标和第二像素坐标，根据所述第一像素坐标和所述第二像素坐标计算视差值，得到第一视差值。

步骤S202、根据所述第一视差值、双目相机的焦距以及双目相机的基线长度，计算所述第i个地图点对应的特征点的深度值，得到第一深度值。

获取第i个地图点对应的特征点分别在当前左图像帧和当前右图像帧中像素坐标系下的像素坐标，对应得到第一像素坐标和第二像素坐标，根据第一像素坐标和第二像素坐标计算视差值，得到第一视差值，具体为，计算第一像素坐标和第二像素坐标的水平坐标之差，得到第一视差值，即：

（1）

其中，d为第一视差值，和/>分别表示第一像素坐标和第二像素坐标的水平坐标。

根据第一视差值、双目相机的焦距以及双目相机的基线长度，计算第i个地图点对应的特征点的深度值（深度信息），得到第一深度值。

其中，在一种可能的实现方式中，计算双目相机的焦距以及双目相机的基线长度的乘积，以乘积为被除数，计算乘积与第一视差值的商，得到第一深度值，深度值具体计算公式可以如下：

（2）

其中，为深度值，/>为相机的焦距，/>为相机的基线长度，/>为视差值。

把相机的焦距、相机的基线长度以及第一视差值代入上式（2）中，即可得到第一深度值，具体如下：

其中，为第一深度值，/>为相机的焦距，/>为相机的基线长度，/>为第一视差值。

在另一种可能的实现方式中，若拍摄当前图像帧的相机为RGB-D相机，RGB-D相机就是特指通过物理方式测距的，能够得到的是RGB图（即RGB-D相机拍摄的图像）加上深度图，可以通过RGB图中特征点在像素坐标系下的像素坐标在深度图中找到对应的深度信息。即当当前图像帧为RGB-D相机所拍摄，则根据第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定第i个地图点对应的第一深度值的方法如步骤S301所示：

步骤S301、根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，从所述当前图像帧对应的深度图像中确定所述像素坐标对应的深度值，得到第一深度值。

除了需要获取第i个地图点对应的特征点的第一深度值外，还需要获取第i个地图点对应的特征点的第二深度值，具体为，获取第i个地图点在世界坐标系下的三维坐标、当前图像帧在世界坐标系下的三维坐标以及旋转矩阵，其中，该旋转矩阵可以为相机的外参数矩阵，根据第i个地图点在世界坐标系下的三维坐标、当前图像帧在世界坐标系下的三维坐标以及旋转矩阵，计算目标坐标，该目标坐标表征由第i个地图点在世界坐标系下的三维坐标、当前图像帧在世界坐标系下的三维坐标以及旋转矩阵反算得到的第i个地图点对应的特征点在当前图像帧中像素坐标系下的二维坐标，相当于一个预估的二维坐标，最后根据目标坐标确定第i个地图点对应的第二深度值。

在一种可能的实现方式中，若当前图像帧为双目相机所拍摄，则当前图像帧包括当前左图像帧和当前右图像帧，根据第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到目标坐标的计算公式为：

其中，表示第i个地图点在世界坐标系下的坐标，/>为第i个地图点的第一深度值，K为双目相机内参数矩阵，R为旋转矩阵，t为当前左图像帧或者当前右图像帧在世界坐标系中的坐标。

目标坐标可以为第一目标坐标或第二目标坐标，若t为当前左图像帧在世界坐标系中的坐标，则（u，v）为第一目标坐标，若t为当前右图像帧在世界坐标系中的坐标，则（u，v）为第二目标坐标。

若（u，v）为第一目标坐标，由于第一目标坐标与第二目标坐标对应于同一个特征点，则可以根据第一目标坐标通过特征点匹配第二目标坐标，或者若（u，v）为第二目标坐标，则可以根据第二目标坐标通过特征点匹配第一目标坐标。该特征点匹配为本技术领域的现有技术手段，此处不详细说明。

若匹配成功，则根据目标坐标确定第i个地图点对应的第二深度值的方法如步骤S401所示：

步骤S401、计算第一目标坐标与第二目标坐标的水平坐标差值，得到第二视差值，根据第二视差值、双目相机的焦距以及双目相机的基线长度，计算第i个地图点对应的特征点的深度值，得到第i个地图点对应的第二深度值。

其中，计算第二深度值与上述计算第二深度值的方法类似，此处不一一赘述。

若匹配失败，则说明地图点无效，将第i个地图点从地图点集合中删除，不用于计算精度。

在另一种可能的实现方式中，若当前图像帧为RGB-D相机所拍摄，则根据第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵计算目标坐标的计算公式如下：

其中，表示第i个地图点在世界坐标系下的坐标，/>为第i个地图点的第一深度值，K为RGB-D相机内参数矩阵，R为旋转矩阵，t为当前图像帧在世界坐标系中的坐标，（u，v）为目标坐标。

根据目标坐标确定第i个地图点对应的第二深度值的方法如步骤S501所示：

步骤S501、从所述当前图像帧对应的深度图像中确定所述目标坐标对应的深度值，得到第二深度值。

上述是对步骤S101进行了说明，下面对步骤S102和步骤S103进行说明。

待得到各个地图点集合中的地图点的第一深度值和第二深度值后，分别计算地图点集中各个地图点对应的第一深度值和第二深度值的差值，得到各个地图点对应的深度差值。

进一步地，根据预设规则分别确定地图点集中各地图点对应的深度差值的权重，具体方法如下：

步骤S601、若所述第i个地图点对应的特征点只存在于所述当前图像帧内，则确定所述第i个地图点对应的深度差值的权重为第一权重值。

步骤S602、若所述第i个地图点对应的特征点存在于所述当前图像帧以及历史图像帧中的关键帧内，并且第i个地图点不为目标地图点，则确定所述第i个地图点对应的深度差值的权重为第二权重值。

若拍摄的相机为双目相机，则当前图像帧包括左当前图像帧和右当前图像帧，则步骤S602可以为若第i个地图点对应的特征点存在于左当前图像帧以及历史左当前图像帧中的关键帧内，并且第i个地图点不为目标地图点，则确定第i个地图点对应的深度差值的权重为第二权重值。

步骤S603、若所述第i个地图点对应的特征点存在于所述当前图像帧以及历史图像帧中的关键帧内，并且第i个地图点为目标地图点，则确定所述第i个地图点对应的深度差值的权重为第三权重值。

若拍摄的相机为双目相机，则当前图像帧包括左当前图像帧和右当前图像帧，则步骤S603可以为若第i个地图点对应的特征点存在于左当前图像帧以及历史左当前图像帧中的关键帧内，并且第i个地图点为目标地图点，则确定第i个地图点对应的深度差值的权重为第三权重值。

其中，历史图像帧为在当前图像帧之前拍摄的图像，目标地图点为根据当前图像帧的位姿以及当前图像帧中特征点的三维坐标更新过地图点信息的地图点，地图点信息至少包括地图点的坐标信息。第三权重值大于第二权重值，第二权重值大于第一权重值。

需要说明是，由于历史图像帧和当前图像帧可能拍摄到相同的物体，因此，历史图像帧和当前图像帧中可能包含了同一个特征点，在视觉SLAM地图中已经存在当前图像帧中的某些特征点所对应的地图点。

此外，在本实施例中，将图像帧中的特征点映射至视觉SLAM地图过程中，会间隔预设时间根据当前图像帧的位姿以及当前图像帧中特征点的三维坐标更新过地图点的信息，其中，特征点的三维坐标可以为世界坐标系下的坐标。对于当前图像帧的拍摄时间而言，有可能正处于地图点信息更新时间段内，也有可能不处于地图点信息更新时间段内，若处于地图点信息更新时间段内，则会根据当前图像帧的位姿以及当前图像帧中特征点的三维坐标，更新该特征点所对应的已存在视觉SLAM地图的地图点的地图点信息。需要说明的是，在本领域中，根据当前图像帧的位姿以及当前图像帧中特征点的三维坐标更新地图点的信息是现有技术，本发明不详细赘述。

基于上述的多种可能情况，确定地图点集中各地图点对应的深度差值的权重，具体为，若第i个地图点对应的特征点只存在于当前图像帧内，则确定第i个地图点对应的深度差值的权重为第一权重值，第一权重值可以取值为1.0。若第i个地图点对应的特征点存在于当前图像帧以及历史图像帧中的关键帧内，并且第i个地图点不为目标地图点，则确定第i个地图点对应的深度差值的权重为第二权重值，第二权重值可以取值为1.5。若第i个地图点对应的特征点存在于当前图像帧以及历史图像帧中的关键帧内，并且第i个地图点为目标地图点，则确定第i个地图点对应的深度差值的权重为第三权重值，第三权重值可以取值为2.0。

由于对于对应的特征点同时存在于当前图像帧以及历史当前图像帧中的关键帧内，且为目标地图点的地图点的认可度，大于对应的特征点同时存在于当前图像帧以及历史图像帧中的关键帧内，且不为目标地图点的地图点，而对对应的特征点只存在于当前图像帧内的地图点认可度最低，因此，根据认可度的高低赋予权重值，可以赋予认可度最高的地图点最大的权重值，赋予认可度最的地图点最大的权重值，可以提高地图点对应的深度差值的有效性，进而提高视觉SLAM位姿估计精度的评估准确性。

根据地图点集中各地图点对应的权重以及深度差值，计算视觉SLAM位姿估计精度的步骤如下：

步骤S701、根据地图点集中所有地图点对应的权重以及深度差值，计算方差累加值，计算地图点集中所有地图点对应的权重之和，得到目标值，根据所述目标值与所述方差累加值计算所述视觉SLAM位姿估计精度。

其中，计算方差累加值的公式如下：

其中，为方差累加值，N为地图点集中的地图点总数，/>为第i个地图点对应的深度差值，/>为第i个地图点对应的权重。

视觉SLAM位姿估计精度计算式如下：

其中，rms为视觉SLAM位姿估计精度，为方差累加值，/>为目标值，/>，N为地图点集中的地图点总数，/>为第i个地图点对应的权重。

在本实施例中，在视觉SLAM位姿估计精度够高的情况下，地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标与目标坐标的几乎相等，因此第一深度值与第二深度值也相差不大，通过根据计算第一深度值与第二深度值的差值得到的深度差值作为计算视觉SLAM位姿估计精度的参数，可以更好地反应精度结果，当深度差值越小时计算出的视觉SLAM位姿估计精度越小，则说明视觉SLAM地图准确度越高。

基于上述方法，可以实现通过视觉SLAM地图中当前图像帧包含的特征点对应的地图点的坐标来确定第一深度值与第二深度值，并根据第一深度值与第二深度值之间的差值以及权重评估视觉SLAM位姿估计精度，不需要依赖外部更高精度传感器获取的地图信息，仅根据自身监测到的内部数据，即地图点信息就可以评估视觉SLAM位姿估计精度，可以有效提高视觉SLAM地图系统精度评估的实用性和可靠性。

为了更好地实现上述方法，本发明实施例提供了一种视觉SLAM位姿估计精度评估装置，该装置可以执行上述方法的全部步骤，参照图2，图2为本发明实施例提供的一种视觉SLAM位姿估计精度评估装置的结构框图，如图2所示，该装置具体如下：

深度值计算模块201：用于获取当前图像帧包含的特征点在视觉SLAM地图中所对应的地图点，得到地图点集，根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第i个地图点对应的第一深度值；根据由所述第i个地图点对应的特征点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第i个地图点对应的第二深度值；其中，i的取值从1至N，N为地图点集中的地图点总数；

深度差值计算模块202：用于计算所述地图点集中第i个地图点的第一深度值和第二深度值的差值，得到所述第i个地图点对应的深度差值；

精度计算模块203：用于根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉SLAM位姿估计精度。

在一种可能的设计中，深度值计算模块201具体用于：若所述当前图像帧为双目相机所拍摄，则所述当前图像帧包括当前左图像帧和当前右图像帧，获取所述第i个地图点对应的特征点分别在当前左图像帧和当前右图像帧中像素坐标系下的像素坐标，对应得到第一像素坐标和第二像素坐标，根据所述第一像素坐标和所述第二像素坐标计算视差值，得到第一视差值；根据所述第一视差值、双目相机的焦距以及双目相机的基线长度，计算所述第i个地图点对应的特征点的深度值，得到第一深度值。

在一种可能的设计中，深度值计算模块201具体用于：计算所述第一像素坐标和所述第二像素坐标的水平坐标之差，得到第一视差值。

在一种可能的设计中，深度值计算模块201具体用于：计算双目相机的焦距以及双目相机的基线长度的乘积；以所述乘积为被除数，计算所述乘积与所述第一视差值的商，得到第一深度值。

在一种可能的设计中，深度值计算模块201具体用于：若所述当前图像帧为双目相机所拍摄，则所述当前图像帧包括当前左图像帧和当前右图像帧，根据所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到目标坐标的计算公式为：

其中，表示第i个地图点在世界坐标系下的坐标，/>为第i个地图点的第一深度值，K为双目相机内参数矩阵，R为旋转矩阵，t为当前左图像帧或者当前右图像帧在世界坐标系中的坐标，若t为当前左图像帧在世界坐标系中的坐标，则（u，v）为第一目标坐标，若t为当前右图像帧在世界坐标系中的坐标，则（u，v）为第二目标坐标；若所述（u，v）为第一目标坐标，则根据所述第一目标坐标通过特征点匹配所述第二目标坐标，或者若所述（u，v）为第二目标坐标，则根据所述第二目标坐标通过特征点匹配所述第一目标坐标；若匹配成功，则计算第一目标坐标与所述第二目标坐标的水平坐标差值，得到第二视差值，根据所述第二视差值、双目相机的焦距以及双目相机的基线长度，计算所述第i个地图点对应的特征点的深度值，得到所述第i个地图点对应的第二深度值；若匹配失败，则将所述第i个地图点从地图点集合中删除。

在一种可能的设计中，深度值计算模块201具体用于：若所述当前图像帧为RGB-D相机所拍摄，则根据所述第i个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，从所述当前图像帧对应的深度图像中确定所述像素坐标对应的深度值，得到第一深度值。

在一种可能的设计中，深度值计算模块201具体用于：若所述当前图像帧为RGB-D相机所拍摄，根据所述第i个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵计算目标坐标；从所述当前图像帧对应的深度图像中确定所述目标坐标对应的深度值，得到第二深度值。

在一种可能的设计中，精度计算模块203具体用于：若所述第i个地图点对应的特征点只存在于所述当前图像帧内，则确定所述第i个地图点对应的深度差值的权重为第一权重值；若所述第i个地图点对应的特征点存在于所述当前图像帧以及历史图像帧中的关键帧内，并且第i个地图点不为目标地图点，则确定所述第i个地图点对应的深度差值的权重为第二权重值；其中，所述目标地图点为根据所述当前图像帧的位姿以及所述当前图像帧中特征点的三维坐标更新过地图点信息的地图点，所述地图点信息至少包括地图点的坐标信息；若所述第i个地图点对应的特征点存在于所述当前图像帧以及历史图像帧中的关键帧内，并且第i个地图点为目标地图点，则确定所述第i个地图点对应的深度差值的权重为第三权重值；其中，所述第三权重值大于所述第二权重值，所述第二权重值大于所述第一权重值。

在一种可能的设计中，精度计算模块203具体用于：根据所述地图点集中所有地图点对应的权重以及深度差值，计算方差累加值；计算所述地图点集中所有地图点对应的权重之和，得到目标值，根据所述目标值与所述方差累加值计算所述视觉SLAM位姿估计精度。

基于上述装置，可以实现不需要依赖外部更高精度传感器获取的地图信息，仅根据自身监测到的内部数据，即地图点信息就可以评估视觉SLAM位姿估计精度，可以有效提高视觉SLAM地图系统精度评估的实用性和可靠性。

图3示出了一个实施例中计算机设备的内部结构图。该计算机设备具体可以是终端，也可以是服务器。如图3所示，该计算机设备包括通过系统总线连接的处理器、存储器和网络接口。其中，存储器包括非易失性存储介质和内存储器。该计算机设备的非易失性存储介质存储有操作系统，还可存储有计算机程序，该计算机程序被处理器执行时，可使得处理器实现上述方法的全部步骤。该内存储器中也可储存有计算机程序，该计算机程序被处理器执行时，可使得处理器执行上述方法的全部步骤。本领域技术人员可以理解，图3中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，提出了一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行前述方法的各个步骤。

在一个实施例中，提出了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行前述方法的各个步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink) DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

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

Claims (10)

1.一种视觉 SLAM 位姿估计精度评估方法，其特征在于，所述方法包括：

获取当前图像帧包含的特征点在视觉 SLAM 地图中所对应的地图点，得到地图点集，根据第 i 个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第i 个地图点对应的第一深度值；根据由所述第 i 个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第 i 个地图点对应的第二深度值；其中，i 的取值从 1 至 N，N 为地图点集中的地图点总数；

计算所述地图点集中第 i 个地图点的第一深度值和第二深度值的差值，得到所述第i 个地图点对应的深度差值；

根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉 SLAM 位姿估计精度。

2.根据权利要求 1 所述的方法，其特征在于，所述根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，包括：

若所述第 i 个地图点对应的特征点只存在于所述当前图像帧内，则确定所述第 i 个地图点对应的深度差值的权重为第一权重值；

若所述第 i 个地图点对应的特征点存在于所述当前图像帧以及历史图像帧中的关键帧内，并且第 i 个地图点不为目标地图点，则确定所述第 i 个地图点对应的深度差值的权重为第二权重值；其中，所述目标地图点为根据所述当前图像帧的位姿以及所述当前图像帧中特征点的三维坐标更新过地图点信息的地图点，所述地图点信息至少包括地图点的坐标信息；

若所述第 i 个地图点对应的特征点存在于所述当前图像帧以及历史图像帧中的关键帧内，并且第 i 个地图点为目标地图点，则确定所述第 i 个地图点对应的深度差值的权重为第三权重值；其中，所述第三权重值大于所述第二权重值，所述第二权重值大于所述第一权重值。

3.根据权利要求 1 所述的方法，其特征在于，所述根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉 SLAM 位姿估计精度，包括：

根据所述地图点集中所有地图点对应的权重以及深度差值，计算方差累加值；

计算所述地图点集中所有地图点对应的权重之和，得到目标值，根据所述目标值与所述方差累加值计算所述视觉 SLAM 位姿估计精度。

4.根据权利要求 1 所述的方法，其特征在于，所述根据第 i 个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第 i 个地图点对应的第一深度值，包括：

若所述当前图像帧为双目相机所拍摄，则所述当前图像帧包括当前左图像帧和当前右图像帧，获取所述第 i 个地图点对应的特征点分别在当前左图像帧和当前右图像帧中像素坐标系下的像素坐标，对应得到第一像素坐标和第二像素坐标，根据所述第一像素坐标和所述第二像素坐标计算视差值，得到第一视差值；

根据所述第一视差值、双目相机的焦距以及双目相机的基线长度，计算所述第 i 个地图点对应的特征点的深度值，得到第一深度值。

5.根据权利要求 4 所述的方法，其特征在于，所述根据所述第一像素坐标和所述第二像素坐标计算视差值，包括：

计算所述第一像素坐标和所述第二像素坐标的水平坐标之差，得到第一视差值。

6.根据权利要求 4 所述的方法，其特征在于，所述根据所述第一视差值、双目相机的焦距以及双目相机的基线长度，计算所述第 i 个地图点对应的特征点的深度值，得到第一深度值，包括：

计算双目相机的焦距以及双目相机的基线长度的乘积；

以所述乘积为被除数，计算所述乘积与所述第一视差值的商，得到第一深度值。

7.根据权利要求 1 所述的方法，其特征在于，所述目标坐标为第一目标坐标或第二目标坐标，所述根据由所述第 i 个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第 i 个地图点对应的第二深度值，包括：

若所述当前图像帧为双目相机所拍摄，则所述当前图像帧包括当前左图像帧

和当前右图像帧，根据所述第 i 个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到目标坐标的计算公式为：

其中，表示第i个地图点在世界坐标系下的坐标，/>为第i个地图点的第一深度值，K为双目相机内参数矩阵，R为旋转矩阵，t为当前左图像帧或者当前右图像帧在世界坐标系中的坐标，若t为当前左图像帧在世界坐标系中的坐标，则（u，v）为第一目标坐标，若t为当前右图像帧在世界坐标系中的坐标，则（u，v）为第二目标坐标；

若所述（u，v）为第一目标坐标，则根据所述第一目标坐标通过特征点匹配所述第二目标坐标，或者若所述（u，v）为第二目标坐标，则根据所述第二目标坐标通过特征点匹配所述第一目标坐标；若匹配成功，则计算第一目标坐标与所述第二目标坐标的水平坐标差值，得到第二视差值，根据所述第二视差值、双目相机的焦距以及双目相机的基线长度，计算所述第i个地图点对应的特征点的深度值，得到所述第i个地图点对应的第二深度值；若匹配失败，则将所述第i个地图点从地图点集合中删除。

8.根据权利要求 1 所述的方法，其特征在于，所述根据第 i 个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第 i 个地图点对应的第一深度值，包括：

若所述当前图像帧为 RGB-D 相机所拍摄，则根据第 i 个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，从所述当前图像帧对应的深度图像中确定所述像素坐标对应的深度值，得到第一深度值。

9.根据权利要求 1 所述的方法，其特征在于，所述根据由所述第 i 个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第 i 个地图点对应的第二深度值，包括：

若所述当前图像帧为 RGB-D 相机所拍摄，根据所述第 i 个地图点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵计算目标坐标；

从所述当前图像帧对应的深度图像中确定所述目标坐标对应的深度值，得到第二深度值。

10.一种视觉 SLAM 位姿估计精度评估装置，其特征在于，所述装置包括：

深度值计算模块：用于获取当前图像帧包含的特征点在视觉 SLAM 地图中所对应的地图点，得到地图点集，根据第 i 个地图点对应的特征点在当前图像帧中像素坐标系下的像素坐标，确定所述第 i 个地图点对应的第一深度值；根据由所述第 i 个地图点对应的特征点在世界坐标系下的坐标、当前图像帧在世界坐标系下的坐标以及旋转矩阵得到的目标坐标，确定所述第 i 个地图点对应的第二深度值；其中，i 的取值从 1 至 N，N 为地图点集中的地图点总数；

深度差值计算模块：用于计算所述地图点集中第 i 个地图点的第一深度值和第二深度值的差值，得到所述第 i 个地图点对应的深度差值；

精度计算模块：用于根据预设规则分别确定所述地图点集中各地图点对应的深度差值的权重，根据所述地图点集中各地图点对应的权重以及深度差值，计算所述视觉 SLAM 位姿估计精度。