CN110335316B - 基于深度信息的位姿确定方法、装置、介质与电子设备 - Google Patents

Abstract

本公开提供了一种基于深度信息的位姿确定方法、位姿确定装置、存储介质与电子设备，属于计算机视觉技术领域。该方法包括：通过相机获取关于场景的当前帧图像，并获取所述当前帧图像的深度信息；从所述当前帧图像提取包含深度信息的特征点；将所述特征点与所述场景的地图模型对齐，以确定所述相机在当前帧的位姿参数。本公开可以提高SLAM中的跟踪精度，且鲁棒性较高。

Description

基于深度信息的位姿确定方法、装置、介质与电子设备

背景技术

SLAM(Simultaneous Localization And Mapping，同时定位和建图)是通过终端设备在场景中移动并采集场景的图像，同时进行设备自身位姿的确定以及场景建模的一种方法，是AR(Augmented Reality，增强现实)、机器人等领域的基础技术。

现有的SLAM方法中，大多从相机拍摄到的场景画面中提取特征，以实时确定相机的位姿，从而将场景画面还原为场景的地图模型。然而，该方法对相机位姿的确定极大地依赖于特征提取的质量，这又取决于场景中的纹理特征是否足够丰富，当遇到弱纹理、无纹理区域或者由于相机快速移动导致画面模糊等情况存在时，会导致提取的特征数目过少、质量过低，影响位姿结果的准确度，导致无法对相机进行精准的跟踪，不利于场景建模。

需要说明的是，在上述背景技术部分公开的信息仅用于加强对本公开的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

发明内容

本公开提供了一种基于深度信息的位姿确定方法、基于深度信息的位姿确定装置、计算机可读存储介质与电子设备，进而至少在一定程度上改善现有的SLAM方法中跟踪精度较低的问题。

本公开的其他特性和优点将通过下面的详细描述变得显然，或部分地通过本公开的实践而习得。

根据本公开的第一方面，提供一种基于深度信息的位姿确定方法，包括：通过相机获取关于场景的当前帧图像，并获取所述当前帧图像的深度信息；从所述当前帧图像提取包含深度信息的特征点；将所述特征点与所述场景的地图模型对齐，以确定所述相机在当前帧的位姿参数。

根据本公开的第二方面，提供一种基于深度信息的位姿确定装置，所述装置包括：图像获取模块，用于通过相机获取关于场景的当前帧图像，并获取所述当前帧图像的深度信息；特征点提取模块，用于从所述当前帧图像提取包含深度信息的特征点；特征点对齐模块，用于将所述特征点与所述场景的地图模型对齐，以确定所述相机在当前帧的位姿参数。

根据本公开的第三方面，提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任意一种基于深度信息的位姿确定方法。

根据本公开的第四方面，提供一种电子设备，包括：处理器；以及存储器，用于存储所述处理器的可执行指令；其中，所述处理器配置为经由执行所述可执行指令来执行上述任意一种基于深度信息的位姿确定方法。

本公开具有以下有益效果：

从具有深度信息的当前帧图像中提取特征点，将特征点与场景的地图模型对齐，以确定相机在当前帧的位姿参数。一方面，当前帧图像的深度信息可以丰富图像信息内容，增加特征点的信息维度，有利于提高特征点的数量与质量，从而在后续得到准确的位姿参数，实现精准的跟踪。另一方面，适用于场景中纹理特征稀缺、画面模糊等情况，可以提高SLAM方法在不同场景下的稳定性与鲁棒性。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施方式，并与说明书一起用于解释本公开的原理。显而易见地，下面描述中的附图仅仅是本公开的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出本示例性实施方式中一种SLAM系统的架构示意图；

图2示出本示例性实施方式中一种基于深度信息的位姿确定方法的流程图；

图3示出本示例性实施方式中一种基于深度信息的位姿确定方法的子流程图；

图4示出本示例性实施方式中另一种基于深度信息的位姿确定方法的流程图；

图5示出本示例性实施方式中另一种基于深度信息的位姿确定法的子流程图；

图6示出本示例性实施方式中一种SLAM方法的流程图；

图7示出本示例性实施方式中一种基于深度信息的位姿确定装置的结构框图；

图8示出本示例性实施方式中一种用于实现上述方法的计算机可读存储介质；

图9示出本示例性实施方式中一种用于实现上述方法的电子设备。

具体实施方式

现在将参考附图更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些实施方式使得本公开将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施方式中。在下面的描述中，提供许多具体细节从而给出对本公开的实施方式的充分理解。然而，本领域技术人员将意识到，可以实践本公开的技术方案而省略所述特定细节中的一个或更多，或者可以采用其它的方法、组元、装置、步骤等。在其它情况下，不详细示出或描述公知技术方案以避免喧宾夺主而使得本公开的各方面变得模糊。

此外，附图仅为本公开的示意性图解，并非一定是按比例绘制。图中相同的附图标记表示相同或类似的部分，因而将省略对它们的重复描述。附图中所示的一些方框图是功能实体，不一定必须与物理或逻辑上独立的实体相对应。可以采用软件形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中实现这些功能实体，或在不同网络和/或处理器装置和/或微控制器装置中实现这些功能实体。

附图中所示的流程图仅是示例性说明，不是必须包括所有的步骤。例如，有的步骤还可以分解，而有的步骤可以合并或部分合并，因此实际执行的顺序有可能根据实际情况改变。

本公开中的“第一”、“第二”、“第三”等仅作为特定对象的标记而使用，并非对数量或顺序等进行限定。

本公开的示例性实施方式首先提供一种基于深度信息的位姿确定方法，该位姿确定方法主要应用于SLAM场景中，确定相机的位姿。图1示出了该方法应用环境的SLAM系统架构图。如图1所示，该SLAM系统100可以包括：场景101、可移动的相机102、可移动的深度传感器103和计算设备104。其中，场景101为待建模的现实场景，如室内、庭院、街道等。相机102和深度传感器103可以一体化设置，例如：相机102为平面摄像头，深度传感器103为设置在旁边的TOF(Time Of Flight，飞行时间)传感器；或者相机102和深度传感器103为两个摄像头，组成双目相机；或者深度传感器103为红外光装置，其与相机102组成结构光相机。相机102和深度传感器103可在场景101内移动，采集场景101的图像及其深度信息，图1示出相机102和深度传感器103设置在可移动的机器人上，此外也可以是用户手持手机或佩戴智能眼镜等在场景101内移动，该手机或智能眼镜内置有相机102和深度传感器103。计算设备104可以是终端计算机或服务器等，与相机102、深度传感器103通讯连接，可进行数据交互，相机102和深度传感器103将采集的图像及其深度信息发送到计算设备104，计算设备104进行处理分析，以实现SLAM中的定位和建模。

需要说明的是，图1所示的SLAM系统100仅是一个示例，还可以有若干的变化形式，举例说明：相机102、深度传感器103和计算设备104可以集成于一个设备中，如内置了相机102、深度传感器103和计算设备104的机器人，其可以在场景101内一边移动一边拍照，并对照片进行处理，实现定位和建模；各装置的数目也不限于图1所示的情况，如可以设置多个相机(如手机上设置3个或4个摄像头)，或者设置多台服务器组成的计算设备集群，通过云计算的方式对大量的场景图像进行处理，等等；还可以增加图中1未示出的装置，如设置与相机102配套的IMU(Inertia Measurement Unit，惯性测量单元)，以帮助确定相机102的位姿，或者设置投影装置，在场景103内产生虚拟投影，与相机102、用户或机器人形成交互。

在SLAM的起始时刻，从零开始对场景建模，此时未采集任何场景图像；SLAM流程开始后，相机跟随用户或者机器人在场景内移动，一边移动一边采集场景图像，形成连续帧的图像流，实时发送到计算设备；在获得一定帧数的图像后，计算设备可以初始化场景的地图模型，通常只包括场景的一小部分，或者与实际场景有差别；此后，相机每采集一帧的图像，计算设备都可以根据该图像对地图模型进行更新优化(当然也可以在筛选出关键帧时进行地图模型的更新优化)，增加地图模型中没有的地图点，或者修正已有地图点的位置等；在进行地图模型的更新优化时，需要先确定相机的位姿参数，这在SLAM中是必需的一个环节，只有在确定相机的位姿参数后，才可以将相机采集的图像对应匹配到三维的地图模型中，以更新优化地图模型。

本示例性实施方式即是针对于如何在每一帧确定相机的位姿参数，所提出的改进方法。基于图1的SLAM系统100，本示例性实施方式的执行主体可以是其中的计算设备104。图2示出了本示例性实施方式的一种流程，可以包括以下步骤S210～S230：

步骤S210，通过相机获取关于场景的当前帧图像，并获取当前帧图像的深度信息。

其中，相机每采集一帧图像，计算设备分析一帧图像，当前帧图像是相机所采集的最新一帧图像。深度信息是与当前帧图像同时采集的，例如：采用平面摄像头加深度传感器拍摄场景图像，可以得到图像中每个像素点的深度信息，通常为深度值；采用双目相机拍摄场景图像，可以通过三角化算法得到图像中每个像素点的深度信息；采用结构光相机拍摄场景图像，可以利用红外点阵向场景投射红外光信号，并在接收反射回来的信号后，通过红外光的变化计算得到深度信息，等等。

步骤S220，从当前帧图像提取包含深度信息的特征点。

其中，特征点是图像中具有代表性的、辨识度较高的点或区域，例如图像中的角点、边缘和一些区块等，本示例性实施方式可以采用ORB算法(Oriented FAST and RotatedBRIEF，面向FAST(Features From Accelerated Segment Test，基于加速分割检测的特征)算法和旋转BRIEF(Binary Robust Independent Elementary Features，二进制鲁棒独立基本特征))提取特征点并对特征点进行描述；也可以采用FAST、SIFT(Scale-InvariantFeature Transform，尺度不变特征变换)、SURF(Speeded Up Robust Features，加速鲁棒特征)等算法提取特征点；还可以对当前帧图像进行目标检测，在检测到的物体边缘轮廓上提取一定的特征点，等等。提取的特征点为当前帧图像中的像素点，具有深度信息，具体而言，每个特征点可以包含其在图像平面中的二维坐标，以及深度方向上的第三维坐标，因而也可以将每个特征点转换为当前帧相机坐标系中的三维坐标。

步骤S230，将特征点与场景的地图模型对齐，以确定相机在当前帧的位姿参数。

其中，场景的地图模型是根据当前帧之前的帧已经建立的地图模型。对于当前帧，存在以下两种情况：当前帧图像所拍摄的为已探明的场景区域，即当前帧图像为地图模型的一个局部；当前帧图像从上一帧向未探明的场景区域方向移动，拍摄到了一部分未探明的场景区域，但大部分区域是与上一帧重合的，即当前帧图像较大比例的与地图模型的一个局部重合。因此，在经过适当的位姿变换的情况下，可以将当前帧图像的特征点与地图模型对齐，通常是与地图模型的一个表面模型对齐，从而基于此可以确定相机的位姿参数。

下面就如何通过特征点与地图模型对齐的方式确定相机的位姿参数，以两个方案进行具体说明。应当理解，下述内容不应对本公开的保护范围造成限定。

方案一、如图3所示，步骤S230可以通过以下步骤S310～S340实现：

步骤S310，预测当前帧图像的特征点在上一帧图像中的位置；

步骤S320，根据地图模型对上一帧图像进行修正；

步骤S330，基于修正后的上一帧图像、特征点在当前帧图像中的位置和在上一帧图像中的位置，构建第一误差函数；

步骤S340，通过计算第一误差函数的最小值，确定相机在当前帧的位姿参数。

其中，根据当前帧之前连续多帧的姿态变化，可以预测上一帧与当前帧之间的姿态变化，表示如下：

k表示当前帧，k-1为上一帧，k-2为上上一帧，w表示世界坐标系。

和

分别表示k-1帧和k-2帧的姿态参数(从世界坐标系到图像平面的姿态变化参数)，

为预测的k帧到k-1帧的姿态变化矩阵。基于此，实际上得到了上一帧与当前帧之间的运动轨迹，从而可以将当前帧图像的特征点对应到上一帧图像中，确定其在上一帧图像中的位置。以V_k表示k帧图像，V_k(x)表示k帧图像中的特征点x的坐标(通常是k帧相机坐标系中的坐标)，x在k-1帧图像中的预测位置如下：

其中，

表示x在k-1帧图像中的预测位置，为图像平面中的像素坐标，π()表示三维点重投影到平面的过程。

本示例性实施方式中，每一帧图像都具有深度信息，包括上一帧图像，上一帧图像中的像素点(主要是特征点)包括三维坐标信息，但是考虑到相机的拍摄误差、噪声、畸变、深度传感器的误差等，该信息与实际场景之间可能存在一定的偏差，可以根据地图模型进行修正。例如：根据k-1帧的姿态参数，将地图模型投影到k-1帧的相机坐标系中，得到修正的k-1帧图像；或者将k-1帧图像投影到世界坐标系中，与地图模型进行平均或加权平均计算，再反向投影回k-1帧的相机坐标系，得到修正的k-1帧图像。可以用V_k-1表示k-1帧图像，

表示通过地图模型修正后的k-1帧图像。修正后的k-1帧图像可以看做是地图模型在k-1帧的表面模型，该表面模型可以被认为是地图模型的所有表面模型中，与当前帧图像最为接近、重合度最高的一个。

根据上述几部分参数，特别是修正后的k-1帧图像，可以构建第一误差函数，表示如下：

其中，

为k帧的姿态参数，

为法向量，其计算方法如下：

norm()表示求单位向量。实际上，第一误差函数表示的是将当前帧图像与经过地图模型修正的上一帧图像进行特征点的对齐而产生的误差，通过计算第一误差函数的最小值，可以优化求解相机在当前帧的位姿参数，如下所示：

在SLAM中，误差函数也可称为优化函数、约束函数等，其用于优化求解相应的变量参数。本示例性实施方式中，通过对第一误差函数进行非线性优化，多次迭代后求得位姿参数

其中，迭代收敛的条件可以是迭代达到一定的轮数，或者第一误差函数在连续两轮迭代中降低的值低于预定的数值等。

方案二、当前帧图像中的特征点具有深度信息，因此可以将特征点表示为三维点云，称为当前帧的三维点云，然后将该三维点云在地图模型中进行匹配，地图模型中应当存在一个局部模型，与当前帧的三维点云具有较高的匹配度。如果无法准确的匹配，可以先大致的匹配出一个局部模型，称为初始局部模型，然后通过当前帧的三维点云与初始局部模型进行位姿优化，具体过程如下：

将初始局部模型降采样到与当前帧的三维点云同样的点数量，这样获得了两个元素数量相同的三维点云模型；

通过ICP(Iterative Closest Point，迭代最近邻点)算法将这两个三维点云模型进行迭代匹配，可以通过SVD(Singular Value Decomposition，奇异值分解)等算法进行迭代，直到收敛；收敛的条件可以是两个三维点云模型之间的匹配误差无法进一步迭代优化，或者达到预设的迭代轮数等，从而得到三维点云与初始局部模型之间的误差，以及优化的位姿参数，称该误差为该位置(局部模型所在位置)的最小误差；

在地图模型中，从初始局部模型向一个方向少量移动，得到移动后的局部模型，并执行上述过程，直到ICP算法收敛后，得到三维点云与移动后的局部模型之间的误差，即对应位置的最小误差，以及优化的位姿参数，若最小误差相对于初始局部模型的最小误差相比有改善，则保留该位姿参数，并继续沿该方向移动，重复以上过程；

若误差没有改善，则改变移动方向，重复以上过程；

在一定范围内，向各个方向移动局部模型，并计算每个位置的最小误差，可以优化出匹配误差最小的位姿参数，将其最终确定为相机在当前帧的位姿参数。

需要补充的是，也可以同时采用上述方案一和方案二，得到两个位姿参数，然后通过加权平均等方式得到最终优化的位姿参数。

基于上述内容，本示例性实施方式中，从具有深度信息的当前帧图像中提取特征点，将特征点与场景的地图模型对齐，以确定相机在当前帧的位姿参数。一方面，当前帧图像的深度信息可以丰富图像信息内容，增加特征点的信息维度，有利于提高特征点的数量与质量，从而在后续得到准确的位姿参数，实现精准的跟踪。另一方面，本示例性实施方式适用于场景中纹理特征稀缺、画面模糊等情况，可以提高SLAM方法在不同场景下的稳定性与鲁棒性。

在一种可选的实施方式中，参考图4所示，在步骤S220后，还可以执行以下步骤S221～S223：

步骤S221，将特征点与局部地图点进行匹配。

其中，局部地图点是指以当前帧图像所拍摄区域为中心的一个局部范围内、在当前帧之前已检测到的场景的地图点，地图点是指在场景的地图模型中已添加的点。在SLAM采集场景图像时，通常从连续帧图像中选取一定数量的关键帧，这是为了减少建模过程中的信息冗余而选取的具有代表性的帧，通常可以每间隔一定的帧数选取一帧为关键帧，或者在图像内容变化较多时提取关键帧。本示例性实施方式中，局部地图点可以是上一关键帧以及上一关键帧的共视关键帧中出现过的地图点。其中，上一关键帧为距离当前帧最近的关键帧；共视是指两帧图像的内容相似度较高，或者具有共同的视场角(Field OfVision，FOV)，说明两帧图像所拍摄的区域重合度较高，具有共视关系，其中一帧为另一帧的共视帧。本示例性实施方式可以检测其他关键帧与上一关键帧的特征点是否为相同的点，若相同的特征点数量超过一定的比例，则该其他关键帧为上一关键帧的共视关键帧，也可以根据相同特征点的数量确定每个其他关键帧与上一关键帧的共视程度，从高到低选取一定数量的其他关键帧，为上一关键帧的共视关键帧。在确定上一关键帧及其共视关键帧后，对其中的地图点取并集，得到的地图点为局部地图点。

在得到局部地图点后，将当前帧图像中的特征点与局部地图点进行匹配，若判断特征点和局部地图点为场景中的同一点，则匹配成功。匹配方法可以包括几种示例性方法：对特征点和局部地图点分别进行特征描述，例如通过ORB、BRIEF等算法，根据描述子的相似度确定特征点和局部地图点是否匹配；对局部地图点进行下采样，使其与当前帧图像中的特征点数量相同，然后对特征点的点云与局部地图点的点云进行ICP算法匹配；基于目标检测从当前帧图像提取的特征点，以物体为单位与局部地图点中的物体模型进行匹配，将匹配成功的物体中的全部特征点与对应的物体模型中的局部地图点进行匹配。

步骤S222，如果匹配成功的特征点数量达到预设数量阈值，则基于匹配成功的特征点及其对应的局部地图点，确定相机在当前帧的位姿参数；

步骤S223，如果匹配成功的特征点数量未达到预设数量阈值，则执行步骤S230。

其中，预设数量阈值可以根据经验或实际应用需求设定，若匹配成功的特征点数量较多，说明特征点整体质量较高，或者特征点与局部地图点之间的偏差较小，基于此，可以根据点云匹配的方式优化特征点与局部地图点之间的误差，得到相机的位姿参数。反之，若匹配成功的特征点数量较少，则通过地图模型对齐的方式具有更高的准确度。

在一种可选的实施方式中，提取特征点后，还可以将深度信息有效的特征点确定为三维特征点，将深度信息无效的特征点确定为二维特征点，其中，三维特征点具有三维位置信息，二维特征点具有二维位置信息。考虑到深度传感器、双目相机等深度检测部件本身的能力限制，无法准确检测距离深度传感器过近或者过远的物体的深度信息，或者对于黑色材质或高反光材质的物体、光照变化较大的场景等处理能力较差，当前帧图像的深度信息可能包含无效的像素点深度信息。因此，基于深度信息是否有效，可以将特征点划分为三维特征点和二维特征点：深度信息无效的特征点为二维特征点，由于其深度信息无效，仅保留其在当前帧图像中的二维坐标(即平面坐标)；深度信息有效的特征点为三维特征点，除了其在当前帧图像中的二维坐标外，还具有其在深度方向上的第三维坐标，其坐标值通常为深度值。

检测深度信息是否有效时，主要检测各特征点的深度信息能否准确反映所拍摄物体的实际情况，基于该思想原理，对于不同类型、不同场景下的图像，所采用的检测方法与标准可以因情况而异，本公开对此不做限定，以下提供几个具体的检测方法示例：

(1)深度传感器在无法准确检测物体深度信息的情况下，可以将对应部分的深度值输出为无效值或异常值，例如TOF传感器的深度检测范围通常为0.5～3米，如果物体到TOF传感器的距离在此范围之外，导致TOF传感器感应到的TOF(发射与接收信号之间的时间差)超出上限或下限，则可以将物体的深度值记为无效值或上限、下限值，因此该深度值不可信，为无效信息；反之，若深度值为检测范围内的正常数值，则为有效信息。

(2)基于目标检测所提取的特征点，可以以物体为单位，对当前帧图像中每个物体的全部特征点进行统一检测，检测该物体的深度值跨度(即最大深度值减去最小深度值)，如果在正常范围内，则该物体的全部特征点的深度信息都有效。例如：从当前帧图像中检测到椅子，并从椅子的轮廓上提取10个特征点(包括角点、边缘点等)，10个特征点中的最大深度值减去最小深度值得到椅子的深度值跨度，认为是该椅子在深度方向上的厚度；通过事先设定每种物体的厚度范围，如椅子的厚度为0.5～2米，则判断上述深度值跨度是否在该范围内，若在，则10个特征点的深度信息全部有效，反之则全部无效。

基于二维特征点和三维特征点的划分，在步骤S222中，可以通过以下步骤确定相机在当前帧的位姿参数：

构建第二误差函数，第二误差函数包括二维误差项和三维误差项，二维误差项为二维特征点及其对应的局部地图点之间的重投影误差，三维误差项为三维特征点及其对应的局部地图点之间的ICP误差；

通过计算第二误差函数的最小值，确定相机在当前帧的位姿参数。

上述步骤可以通过以下公式表示：

其中，Loss2为第二误差函数，Loss2D为二维误差项，Loss3D为三维误差项；P_2D表示二维特征点的集合，e_i,k表示其中的任一点与对应的局部地图点之间的误差；P_3D表示三维特征点的集合，e_j,k表示其中的任一点与对应的局部地图点之间的误差；k表示当前帧。以重投影误差表示二维误差项，以ICP误差表示三维误差项，则有：

其中，

为二维特征点i在当前帧图像中的平面坐标，P₄ ^w为二维特征点i对应的局部地图点的世界坐标，π()表示三维的局部地图点投影到图像平面(此处为当前帧图像的平面)，因此，公式(7)表示将局部地图点重投影到当前帧图像的平面后，与对应的二维特征点之间的平面坐标误差。类似的，在公式(8)中，

为三维特征点j在当前帧图像中的立体坐标(包含深度信息，为三维的相机坐标系中的坐标)，P₈ ^w为三维特征点j对应的局部地图点的世界坐标，经过

转换到相机坐标系中，与

计算坐标误差。

在一种可选的实施方式中，可以在第二误差函数中添加鲁棒核函数ρ()，用于减少误匹配对最终结果的影响，如下所示：

在一种可选的实施方式中，还可以在第二误差函数中添加信息矩阵，用于衡量特征点的观测不确定性，如下所示：

其中，

是以协方差矩阵的形式表示的二维特征点i的信息矩阵，

表示的三维特征点j的信息矩阵，信息矩阵与相机本身的噪声性能等相关，在此相当于对不同位置的特征点进行了加权计算，可以提高第二误差函数的准确度。

通过对第二误差函数进行非线性优化，多次迭代后求得位姿参数

表示如下：

可以将公式(5)和公式(11)看作是优化位姿参数的两种途径，在匹配成功的特征点数量较多时，采用公式(11)，反之采用公式(5)。因此本示例性实施方式具有较高的灵活性。

在一种可选的实施方式中，如果预先对IMU和视觉信号单元进行对准(或称为对齐、配准、融合、耦合等)，则第二误差函数还可以包括惯性测量误差项，为IMU和视觉信号单元之间的误差。其中，视觉信号单元是指通过视觉信号(主要为图像)进行定位与建模的单元，主要包括相机，还可以包括与相机配套的深度传感器、计算机等。第二误差函数可以如下所示：

引入鲁棒核函数ρ()和信息矩阵的情况下，第二误差函数也可以是：

在公式(12)和(13)中，e_IMU,k为惯性测量误差项，表示在当前帧IMU和视觉信号单元之间的误差，

表示IMU的信息矩阵。第二误差函数中设置惯性测量误差项，可以将IMU信号作为位姿优化的一项依据参数，进一步提高位姿参数的准确度。

本示例性实施方式中，IMU和视觉信号单元的对准可以包括以下步骤：通过计算IMU的预积分中的旋转参数和视觉信号单元测量的旋转参数之间的误差最小值，得到IMU的陀螺仪偏置；通过计算IMU的预积分中的位置参数和视觉信号单元测量的位置参数之间的误差最小值，得到IMU的重力加速度；基于IMU的陀螺仪偏置和重力加速度，将IMU和视觉信号单元进行对准。上述步骤可以在SLAM的初始化阶段执行，即在初始化阶段使IMU和视觉信号单元对准，此后在跟踪过程中，也可以执行上述步骤，不断优化调整IMU和视觉信号单元的对准状态，以进一步提高跟踪的精度。

需要补充的是，在通过计算第二误差函数的最小值，确定相机在当前帧的位姿参数后，还可以统计重投影误差小于第一阈值的二维特征点的数量，以及ICP误差小于第二阈值的三维特征点的数量，两数量相加得到跟踪成功的特征点数量，如果该数量达到第一预设数量，则确定当前得到的位姿参数有效；反之，则确定当前得到的位姿参数无效，执行步骤S230以确定位姿参数。其中，第一阈值、第二阈值和第一预设数量都可以根据经验或实际应用需求设定。通过计算第二误差函数的最小值确定位姿参数后，将位姿参数带入每组特征点与对应局部地图点的点对，计算其误差：对于二维特征点，将局部地图点通过位姿参数重投影到当前帧图像的平面，计算与二维特征点的重投影误差；对于三维特征点，则计算其与对应的局部地图点之间的ICP误差；对于两类特征点，采用不同的衡量标准，即第一阈值和第二阈值，误差达到或超过衡量标准时，认为该特征点跟踪不成功。从而可以统计出跟踪成功的特征点数量，若小于第一预设数量，则认为当前帧整体的跟踪状况不理想，当前得到的位姿参数准确度无法保证，因此采用步骤S230的方式重新确定位姿参数。

上述跟踪以及位姿确定的过程通常由SLAM中的跟踪线程负责执行，此外，SLAM还可以包括三维重构线程(或称为地图建模线程、关键帧处理线程等)，主要用于构建优化三维的地图模型。

在一种可选的实施方式中，确定相机在当前帧的位姿参数后，还可以在判断当前帧满足第一预设条件时，将当前帧确定为新的关键帧，由三维重构线程根据新的关键帧以及位姿参数更新场景的地图模型；在判断当前帧不满足第一预设条件时，将当前帧确定为普通帧，进入下一帧的处理。

其中，如果位姿参数是通过步骤S230确定的，则第一预设条件可以包括：当前帧距离上一关键帧超过预设帧数，上一关键帧为距离当前帧最近的关键帧；预设帧数可根据经验或实际应用需求设定，例如超过15帧则当前帧为新的关键帧。

如果位姿参数是通过步骤S222确定的，则第一预设条件可以包括：

当前帧距离上一关键帧超过预设帧数；

当前帧与上一关键帧的视差超过预设差值，视差为共视的相反概念，表示两帧所拍摄的区域的差异程度，差异越大，共视程度越低，视差越大；预设差值可根据经验或实际应用需求设定，例如可以设定为15％，当前帧与上一关键帧的视差超过15％时为新的关键帧。

统计当前帧图像中，与局部地图点之间的误差小于第一阈值的二维特征点的数量，以及与局部地图点之间的误差小于第二阈值的三维特征点的数量，两数量之和为当前帧跟踪成功的特征点数量，若小于第二预设数量，则当前帧为新的关键帧。第一阈值、第二阈值和第二预设数量可根据经验或实际应用需求设定。第二预设数量可以大于第一预设数量。

需要说明的是，也可以任意组合上述3种条件，例如当前帧距离上一关键帧超过预设帧数，且当前帧与上一关键帧的视差超过预设差值时，当前帧为新的关键帧，本公开对此不做限定。

在确定新的关键帧后，可以将其加入关键帧队列，三维重构线程对队列中的关键帧依次处理，以更新场景的地图模型。

在更新地图模型时，根据位姿参数确定的方式，可以将新的关键帧分为深度关键帧(对应于步骤S230所确定的位姿参数)与特征点关键帧(对应于步骤S222所确定的位姿参数)两类情况，分别采用不同的更新方式，下面分别说明：

一、新的关键帧为深度关键帧，可以根据位姿参数将该关键帧转换到世界坐标系中，并通过TSDF(Truncated Signed Distance Function，截断符号距离场)算法将转换后的新的关键帧加权融合至地图模型中，以更新地图模型。具体而言，对于地图模型的每个空间体素v，其TSDF采用D(v)表示，权重用W(v)表示；将新的关键帧D_k融合到地图模型中，对于图像中的每个有效像素，采用如下的方法更新对应的体素以及权重：

WF(v)＝W(v)+W_k(v)；  (15)

其中，DF(v)和WF(v)分别为更新后的体素和权重，通过这样加权计算的方式，可以得到更新后的地图模型。

二、新的关键帧为特征点关键帧，从以下三个方面进行说明：

第1方面、可以对已有的地图点进行更新，同时进一步优化关键帧的位姿参数，参考图5所示，具体包括以下步骤S510～S540：

步骤S510，获取新的关键帧以及与新的关键帧关联的其他关键帧，形成关键帧集合。

其中，与新的关键帧关联的其他关键帧可以是：距离新的关键帧最近的M个关键帧，以及新的关键帧的N个共视关键帧，M和N是预设的正整数，可根据经验或实际应用需求设定，当然M个关键帧和N个共视关键帧中可能有重复的帧，两部分取并集即可，得到关键帧集合，记为F_key。或者也可以将与新的关键帧具有其他关联关系的关键帧形成关键帧集合F_key。

步骤S520，获取关键帧集合中出现过的全部地图点，形成地图点集合。

换而言之，对F_key中全部关键帧的地图点取并集，形成地图点集合，记为P_map。

步骤530，基于关键帧集合与其中每个关键帧的位姿参数、以及地图点集合，构建第三误差函数。

其中，第三误差函数包括重投影误差项，重投影误差项为地图点集合中的任一地图点到关键帧集合中的任一关键帧的重投影误差之和，可以表示如下：

e_o,p表示P_map中的任一地图点p到F_key中的任一关键帧o的重投影误差。进一步的，也可以在第三误差函数中添加鲁棒核函数ρ()，则有：

在一种可选的实施方式中，为了提高第三误差函数的准确度，还可以设置帧间惯性测量误差项，为IMU在关键帧集合中任意两相邻关键帧i和i+1之间的误差之和，如下所示：

在公式(18)中，还添加了关键帧i和i+1之间的IMU信息矩阵，可以进一步优化第三误差函数。

步骤S540，通过计算第三误差函数的最小值，优化关键帧集合中各关键帧的位姿参数以及地图点集合中各地图点的坐标，以更新地图模型。

其中，优化求解可以如下所示：

X^N为P_map中的任一地图点p的世界坐标，

为F_key中的任一关键帧q的位姿参数，通过优化求解这两项参数，可以优化修正地图点坐标，更新地图模型。

第2方面、可以删除已有的地图点中异常的地图点，具体而言，基于上述建立的关键帧集合和地图点集合，可以将满足第二预设条件的地图点作为异常地图点，从地图模型中删除。第二预设条件包括以下任一条：

若地图点p在关键帧集合中各关键帧上的重投影误差的均值大于预设误差阈值，则p为异常地图点。预设误差阈值可根据经验或实际应用需求设定。在计算重投影误差时，可以选取关键帧集合中的全部关键帧进行计算，也可以选取p具有投影的关键帧进行计算。

若地图点p在关键帧集合中被跟踪成功的关键帧数量小于预测数量乘以预设比例，则p为异常地图点。被跟踪成功是指地图点p在关键帧上的重投影误差小于一定的数值，例如小于上述第一阈值。基于p的位置以及各关键帧的位姿参数，关键帧处理线程可以预测p被跟踪成功的关键帧数量，该数量乘以小于或等于1的预设比例，结果用于衡量是否跟踪异常；预设比例表示允许的偏差程度，可根据经验或实际应用需求设定，例如为0.5。判断关系如下所示：

c()为指示函数，在()内为真和假时分别取值为1和0；T1为第一阈值，R为预设比例，Pre()表示预测函数。

第3方面、可以增加新的地图点，具体而言，若新的关键帧中存在与局部地图点(或地图点集合中的地图点)不匹配的特征点，可以认为该特征点尚不存在于地图模型中；于是可以将该特征点与关键帧集合中其他关键帧的特征点进行匹配，若匹配成功，得到一对特征点点对，可以认为该点对是场景中的同一点在两个不同帧上的投影；对该点对进行三角化计算，以还原出其在场景中的三维坐标，从而得到一个新的地图点，可以添加到地图模型中。

需要补充的是，上述方法实际可以应用于每个关键帧，将关键帧集合中每个关键帧的特征点都与地图点集合中的地图点进行匹配，未匹配成功的特征点形成一个未知点集合，再将未知点集合中的特征点两两匹配(采用不放回的方式，匹配出一个点对，则将两个点从集合中移除，因此不会出现一个点匹配到两个或多个点的情况)，匹配的点对对应三角化生成新的地图点。

在一种可选的实施方式中，若新的关键帧中存在与局部地图点不匹配的特征点，由于该特征点具有深度信息，即该特征点具有三维的图像坐标，根据关键帧的位姿参数可以将该特征点映射到世界坐标系中，计算出其真实位置，从而作为新的地图点，添加到地图模型中。即使该地图点的位置有偏差，也可以在后续帧的处理中不断优化。

除了上述跟踪线程和关键帧处理线程外，SLAM还可以包括回环检测线程，用于针对新的关键帧进行回环检测，以对地图模型进行全局优化。具体而言，针对新的关键帧，通过预先训练的视觉词袋模型将该关键帧中的特征点转换为词典描述；然后计算该关键帧与之前的关键帧的词典相似性，若相似性达到一定的阈值，则认为该关键帧为候选回环帧；针对候选回环帧做几何验证，即匹配点之间应该满足对应的几何关系，如果通过几何验证则认为是回环帧，进行地图模型的全局优化。

在一种可选的实施方式中，回环检测也可以仅针对新的关键帧为特征点关键帧的情况，若新的关键帧为深度关键帧，则跳过回环检测的环节，进入下一帧的处理，这样能够简化回环检测所需的视觉词袋模型，减少系统任务量。

图6示出了本示例性实施方式的一种SLAM方法流程，包括跟踪线程、三维重构线程和回环检测线程分别执行的3个部分，下面对3个线程所执行的方法流程分别说明：

跟踪线程执行步骤S611，采集当前帧图像及其深度信息；然后执行步骤S612，从当前帧图像提取特征点；然后执行步骤S613，将特征点与局部地图点进行匹配；然后执行步骤S614，判断匹配成功的特征点数量是否足够多，具体为是否达到预设数量阈值；若匹配成功的特征点数量未达到预设数量阈值，则执行步骤S615，利用当前帧图像对齐地图模型的方式优化位姿参数，具体可以构建第一误差函数，通过计算第一误差函数的最小值，优化位姿参数；若匹配成功的特征点数量达到预设数量阈值，则执行步骤S616，利用特征点优化位姿参数，具体可以构建第二误差函数，通过计算第二误差函数的最小值，优化位姿参数；然后执行步骤S617，判断特征点优化的位姿参数是否准确，若不准确，则仍然可以执行步骤S615，利用当前帧图像对齐地图模型的方式优化位姿参数；若准确，则执行步骤S618，判断当前帧是否满足第一预设条件，此外在步骤S615后，也可以执行步骤S618；若当前帧不满足第一预设条件，则进入下一帧的处理，主要是下一帧的位姿确定，重复上述流程；若当前帧满足第一预设条件，则将其作为新的关键帧，加入到关键帧队列。跟踪线程的流程结束。

关键帧处理线程响应于当前帧作为新的关键帧，执行步骤S621，在关键帧队列中添加新的关键帧；然后执行步骤S622，判断该关键帧是否为深度关键帧，深度关键帧是指通过当前帧图像对齐地图模型而优化位姿的情况；若新的关键帧是深度关键帧，则执行步骤S623，对地图模型进行三维重构；若新的关键帧不是深度关键帧，即通过特征点优化的方式确定的位姿参数，则执行步骤S624，局部优化地图模型，具体为通过计算第三误差函数的最小值，优化多个附近关键帧(靠近当前帧的关键帧)的位姿以及地图点的位置；然后执行步骤S625，判断IMU和视觉信号单元是否对准，若未对准，则执行步骤S626以进行对准；在对准前，还可以判断一定数量的附近关键帧之间是否存在明显的视差，如果存在，则可以进行对准，如果不存在，则认为无法对准，跳过对准的步骤；在对准后，执行步骤S627，删除地图模型中的异常地图点；然后执行步骤S628，在地图模型中添加新的地图点；然后可以执行步骤S623，对地图模型进行三维重构。关键帧处理线程的流程结束。

回环检测线程在关键帧处理线程局部优化的基础上，可以进行全局优化，具体包括：先执行步骤S631，进行回环检测；若为回环候选帧，则执行步骤S632，进行几何验证；若几何验证通过，则执行步骤S633，对地图模型全局优化。

本公开的示例性实施方式还提供了一种基于深度信息的位姿确定装置，可以应用于图1所示的SLAM系统中的计算设备104，在SLAM的方法过程中进行位姿确定。如图7所示，该位姿确定装置700可以包括：图像获取模块710，用于通过相机获取关于场景的当前帧图像，并获取当前帧图像的深度信息；特征点提取模块720，用于从当前帧图像提取包含深度信息的特征点；特征点对齐模块730，用于将特征点与场景的地图模型对齐，以确定相机在当前帧的位姿参数。

在一种可选的实施方式中，特征点对齐模块730可以包括：位置预测单元，用于预测特征点在上一帧图像中的位置；图像修正单元，用于根据地图模型对上一帧图像进行修正；第一函数构建单元，用于基于修正后的上一帧图像、特征点在当前帧图像中的位置和在上一帧图像中的位置，构建第一误差函数；位姿优化单元，用于通过计算第一误差函数的最小值，确定相机在当前帧的位姿参数。

在一种可选的实施方式中，特征点对齐模块730，还可以用于将特征点与局部地图点进行匹配，如果匹配成功的特征点数量达到预设数量阈值，则基于匹配成功的特征点及其对应的局部地图点，确定相机在当前帧的位姿参数，如果匹配成功的特征点数量未达到预设数量阈值，则将特征点与场景的地图模型对齐，以确定相机在当前帧的位姿参数。

在一种可选的实施方式中，特征点提取模块720，还可以用于将深度信息有效的特征点确定为三维特征点，将深度信息无效的特征点确定为二维特征点，其中，三维特征点具有三维位置信息，二维特征点具有二维位置信息；特征点对齐模块730，还可以用于构建第二误差函数，第二误差函数包括二维误差项和三维误差项，二维误差项为二维特征点及其对应的局部地图点之间的重投影误差，三维误差项为三维特征点及其对应的局部地图点之间的迭代最近邻误差，并通过计算第二误差函数的最小值，确定相机在当前帧的位姿参数。

在一种可选的实施方式中，局部地图点可以包括：上一关键帧以及上一关键帧的共视关键帧中出现过的地图点；其中，上一关键帧为距离当前帧最近的关键帧。

在一种可选的实施方式中，如果位姿确定装置700预先对惯性测量单元和视觉信号单元进行对准，则第二误差函数还可以包括惯性测量误差项，惯性测量误差项为惯性测量单元和视觉信号单元之间的误差，视觉信号单元主要包括相机。

在一种可选的实施方式中，位姿确定装置700还可以包括：IMU对准模块，用于通过计算惯性测量单元的预积分中的旋转参数和视觉信号单元测量的旋转参数之间的误差最小值，得到惯性测量单元的陀螺仪偏置，通过计算惯性测量单元的预积分中的位置参数和视觉信号单元测量的位置参数之间的误差最小值，得到惯性测量单元的重力加速度，并基于惯性测量单元的陀螺仪偏置和重力加速度，将惯性测量单元和视觉信号单元进行对准。

在一种可选的实施方式中，位姿确定装置700还可以包括：地图更新模块，用于当判断当前帧满足第一预设条件时，将当前帧确定为新的关键帧，并根据新的关键帧以及位姿参数更新场景的地图模型，当判断当前帧不满足第一预设条件时，将当前帧确定为普通帧，进入下一帧的处理；其中，如果位姿参数是通过将特征点与场景的地图模型对齐而确定的，则第一预设条件包括：当前帧距离上一关键帧超过预设帧数，上一关键帧为距离当前帧最近的关键帧；如果位姿参数是基于匹配成功的特征点及其对应的局部地图点而确定的，则第一预设条件包括以下任意一个或多个的组合：当前帧距离上一关键帧超过预设帧数；当前帧与上一关键帧的视差超过预设差值；在当前帧跟踪成功的特征点数量小于第二预设数量，跟踪成功的特征点数量通过以下方法确定：若匹配成功的特征点及其对应的局部地图点之间的误差小于预设值，则该特征点为跟踪成功的特征点，遍历当前帧图像的特征点，统计得到跟踪成功的特征点数量。

在一种可选的实施方式中，如果位姿参数是通过将特征点与场景的地图模型对齐而确定的，则地图更新模块还可以用于根据位姿参数将新的关键帧转换到世界坐标系中，并通过TSDF算法将转换后的新的关键帧加权融合至地图模型中，以更新地图模型。

在一种可选的实施方式中，地图更新模块可以包括：关键帧获取单元，用于获取新的关键帧以及与新的关键帧关联的其他关键帧，形成关键帧集合；地图点获取单元，用于获取关键帧集合中出现过的全部地图点，形成地图点集合；第三函数构建单元，用于基于关键帧集合与其中每个关键帧的位姿参数、以及地图点集合，构建第三误差函数，第三误差函数包括重投影误差项，重投影误差项为地图点集合中的任一地图点到关键帧集合中的任一关键帧的重投影误差之和；优化处理单元，用于通过计算第二误差函数的最小值，优化关键帧集合中各关键帧的位姿参数以及地图点集合中各地图点的坐标，以更新地图模型。

在一种可选的实施方式中，第三误差函数还可以包括帧间惯性测量误差项，为惯性测量单元在关键帧集合中任意两相邻关键帧之间的误差之和。

在一种可选的实施方式中，与新的关键帧关联的其他关键帧可以包括：距离新的关键帧最近的M个关键帧，以及新的关键帧的N个共视关键帧；其中，M和N是预设的正整数。

在一种可选的实施方式中，地图更新模块还可以包括：地图点删除单元，用于将地图点集合中满足第二预设条件的地图点从地图模型中删除；其中，第二预设条件可以包括：地图点在关键帧集合中被跟踪成功的关键帧数量小于预测数量乘以预设比例，预设比例小于或等于1；或者地图点在关键帧集合中各关键帧上的重投影误差的均值大于预设误差阈值。

在一种可选的实施方式中，地图更新模块还可以包括：地图点添加单元，用于若新的关键帧中存在与局部地图点不匹配的特征点，则将该特征点与关键帧集合中其他关键帧的特征点进行匹配，根据匹配的结果进行三角化计算，得到新的地图点，以添加到地图模型中。

在一种可选的实施方式中，位姿确定装置700还可以包括：回环检测模块，用于针对新的关键帧进行回环检测，以对地图模型进行全局优化。

上述装置的模块/单元的具体细节在方法部分实施方式中已经详细说明，未披露的方案细节内容可以参见方法部分的实施方式内容，因而不再赘述。

所属技术领域的技术人员能够理解，本公开的各个方面可以实现为系统、方法或程序产品。因此，本公开的各个方面可以具体实现为以下形式，即：完全的硬件实施方式、完全的软件实施方式(包括固件、微代码等)，或硬件和软件方面结合的实施方式，这里可以统称为“电路”、“模块”或“系统”。

本公开的示例性实施方式还提供了一种计算机可读存储介质，其上存储有能够实现本说明书上述方法的程序产品。在一些可能的实施方式中，本公开的各个方面还可以实现为一种程序产品的形式，其包括程序代码，当程序产品在终端设备上运行时，程序代码用于使终端设备执行本说明书上述“示例性方法”部分中描述的根据本公开各种示例性实施方式的步骤。

参考图8所示，描述了根据本公开的示例性实施方式的用于实现上述方法的程序产品800，其可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码，并可以在终端设备，例如个人电脑上运行。然而，本公开的程序产品不限于此，在本文件中，可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了可读程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质，该可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。

可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、有线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言的任意组合来编写用于执行本公开操作的程序代码，程序设计语言包括面向对象的程序设计语言—诸如Java、C++等，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中，远程计算设备可以通过任意种类的网络，包括局域网(LAN)或广域网(WAN)，连接到用户计算设备，或者，可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。

本公开的示例性实施方式还提供了一种能够实现上述方法的电子设备。下面参照图9来描述根据本公开的这种示例性实施方式的电子设备900。图9显示的电子设备900仅仅是一个示例，不应对本公开实施方式的功能和使用范围带来任何限制。

如图9所示，电子设备900可以以通用计算设备的形式表现。电子设备900的组件可以包括但不限于：上述至少一个处理单元910、上述至少一个存储单元920、连接不同系统组件(包括存储单元920和处理单元910)的总线930和显示单元940。

存储单元920存储有程序代码，程序代码可以被处理单元910执行，使得处理单元910执行本说明书上述“示例性方法”部分中描述的根据本公开各种示例性实施方式的步骤。例如，处理单元910可以执行图2至图6所示的方法步骤等。

存储单元920可以包括易失性存储单元形式的可读介质，例如随机存取存储单元(RAM)921和/或高速缓存存储单元922，还可以进一步包括只读存储单元(ROM)923。

存储单元920还可以包括具有一组(至少一个)程序模块925的程序/实用工具924，这样的程序模块925包括但不限于：操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。

总线930可以为表示几类总线结构中的一种或多种，包括存储单元总线或者存储单元控制器、外围总线、图形加速端口、处理单元或者使用多种总线结构中的任意总线结构的局域总线。

电子设备900也可以与一个或多个外部设备1000(例如键盘、指向设备、蓝牙设备等)通信，还可与一个或者多个使得用户能与该电子设备900交互的设备通信，和/或与使得该电子设备900能与一个或多个其它计算设备进行通信的任何设备(例如路由器、调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口950进行。并且，电子设备900还可以通过网络适配器960与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。如图所示，网络适配器960通过总线930与电子设备900的其它模块通信。应当明白，尽管图中未示出，可以结合电子设备900使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。

通过以上的实施方式的描述，本领域的技术人员易于理解，这里描述的示例实施方式可以通过软件实现，也可以通过软件结合必要的硬件的方式来实现。因此，根据本公开实施方式的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中或网络上，包括若干指令以使得一台计算设备(可以是个人计算机、服务器、终端装置、或者网络设备等)执行根据本公开示例性实施方式的方法。

此外，上述附图仅是根据本公开示例性实施方式的方法所包括的处理的示意性说明，而不是限制目的。易于理解，上述附图所示的处理并不表明或限制这些处理的时间顺序。另外，也易于理解，这些处理可以是例如在多个模块中同步或异步执行的。

应当注意，尽管在上文详细描述中提及了用于动作执行的设备的若干模块或者单元，但是这种划分并非强制性的。实际上，根据本公开的示例性实施方式，上文描述的两个或更多模块或者单元的特征和功能可以在一个模块或者单元中具体化。反之，上文描述的一个模块或者单元的特征和功能可以进一步划分为由多个模块或者单元来具体化。

本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本公开的其他实施方式。本申请旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施方式仅被视为示例性的，本公开的真正范围和精神由权利要求指出。

应当理解的是，本公开并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本公开的范围仅由所附的权利要求来限。

Claims (18)

1.一种基于深度信息的位姿确定方法，其特征在于，包括：

通过相机获取关于场景的当前帧图像，并获取所述当前帧图像的深度信息；

从所述当前帧图像提取包含深度信息的特征点；

将所述特征点与所述场景的地图模型对齐，以确定所述相机在当前帧的位姿参数；

其中，所述将所述特征点与所述场景的地图模型对齐，以确定所述相机在当前帧的位姿参数，包括：

预测所述特征点在上一帧图像中的位置；根据所述地图模型对所述上一帧图像进行修正；基于修正后的所述上一帧图像、所述特征点在所述当前帧图像中的位置和在所述上一帧图像中的位置，构建第一误差函数；通过计算所述第一误差函数的最小值，确定所述相机在当前帧的位姿参数；及/或

根据所述特征点生成所述当前帧的三维点云；通过迭代最近邻点算法将所述地图模型中的局部模型与所述三维点云进行迭代匹配，并通过在所述地图模型中移动所述局部模型，以迭代优化所述三维点云与所述局部模型的匹配误差；将匹配误差最小时的位姿参数作为所述相机在当前帧的位姿参数。

2.根据权利要求1所述的方法，其特征在于，所述通过计算所述第一误差函数的最小值，确定所述相机在当前帧的位姿参数，包括：

对所述第一误差函数进行迭代优化，当达到迭代收敛的条件时，得到所述相机在当前帧的位姿参数；所述迭代收敛的条件包括：迭代达到一定的轮数，或者所述第一误差函数在连续两轮迭代中降低的值低于预定的数值。

3.根据权利要求1所述的方法，其特征在于，在从所述当前帧图像提取包含深度信息的特征点后，所述方法还包括：

将所述特征点与局部地图点进行匹配；

如果匹配成功的特征点数量达到预设数量阈值，则基于匹配成功的特征点及其对应的局部地图点，确定所述相机在当前帧的位姿参数；

如果匹配成功的特征点数量未达到所述预设数量阈值，则执行将所述特征点与所述场景的地图模型对齐，以确定所述相机在当前帧的位姿参数的步骤。

4.根据权利要求3所述的方法，其特征在于，在将所述特征点与局部地图点进行匹配前，所述方法还包括：

将深度信息有效的特征点确定为三维特征点，将深度信息无效的特征点确定为二维特征点，其中，所述三维特征点具有三维位置信息，所述二维特征点具有二维位置信息；

所述基于匹配成功的特征点及其对应的局部地图点，确定所述相机在当前帧的位姿参数，包括：

构建第二误差函数，所述第二误差函数包括二维误差项和三维误差项，所述二维误差项为所述二维特征点及其对应的局部地图点之间的重投影误差，所述三维误差项为所述三维特征点及其对应的局部地图点之间的迭代最近邻误差；

通过计算所述第二误差函数的最小值，确定所述相机在当前帧的位姿参数。

5.根据权利要求4所述的方法，其特征在于，如果预先对惯性测量单元和视觉信号单元进行对准，则所述第二误差函数还包括惯性测量误差项，为所述惯性测量单元和所述视觉信号单元之间的误差，所述视觉信号单元包括所述相机。

6.根据权利要求5所述的方法，其特征在于，所述对惯性测量单元和视觉信号单元进行对准，包括：

通过计算所述惯性测量单元的预积分中的旋转参数和所述视觉信号单元测量的旋转参数之间的误差最小值，得到所述惯性测量单元的陀螺仪偏置；

通过计算所述惯性测量单元的预积分中的位置参数和所述视觉信号单元测量的位置参数之间的误差最小值，得到所述惯性测量单元的重力加速度；

基于所述惯性测量单元的陀螺仪偏置和重力加速度，将所述惯性测量单元和所述视觉信号单元进行对准。

7.根据权利要求4所述的方法，其特征在于，在通过计算所述第二误差函数的最小值，确定所述相机在当前帧的位姿参数后，所述方法还包括：

统计重投影误差小于第一阈值的二维特征点的数量，以及迭代最近邻误差小于第二阈值的三维特征点的数量，两数量相加得到跟踪成功的特征点数量；

如果所述跟踪成功的特征点数量达到第一预设数量，则确定当前得到的所述位姿参数有效；

如果所述跟踪成功的特征点数量未达到所述第一预设数量，则确定当前得到的所述位姿参数无效，并执行将所述特征点与所述场景的地图模型对齐，以确定所述相机在当前帧的位姿参数的步骤。

8.根据权利要求3-7任一项所述的方法，其特征在于，在确定所述相机在当前帧的位姿参数后，所述方法还包括：

当判断所述当前帧满足第一预设条件时，将所述当前帧确定为新的关键帧，并根据所述新的关键帧以及所述位姿参数更新所述场景的地图模型；

当判断所述当前帧不满足所述第一预设条件时，将所述当前帧确定为普通帧，进入下一帧的处理；

其中，如果所述位姿参数是通过将所述特征点与所述场景的地图模型对齐而确定的，则所述第一预设条件包括：

所述当前帧距离上一关键帧超过预设帧数，所述上一关键帧为距离所述当前帧最近的关键帧；

如果所述位姿参数是基于匹配成功的特征点及其对应的局部地图点而确定的，则所述第一预设条件包括以下任意一个或多个的组合：

所述当前帧距离所述上一关键帧超过所述预设帧数；

所述当前帧与所述上一关键帧的视差超过预设差值；

在所述当前帧跟踪成功的特征点数量小于第二预设数量，所述跟踪成功的特征点数量通过以下方法确定：

若匹配成功的特征点及其对应的局部地图点之间的误差小于预设值，则该特征点为跟踪成功的特征点，遍历所述当前帧图像的特征点，统计得到所述跟踪成功的特征点数量。

9.根据权利要求8所述的方法，其特征在于，如果所述位姿参数是通过将所述特征点与所述场景的地图模型对齐而确定的，则所述根据所述新的关键帧以及所述位姿参数更新所述地图模型，包括：

根据所述位姿参数将所述新的关键帧转换到世界坐标系中；

通过截断符号距离场算法将转换后的所述新的关键帧加权融合至所述地图模型中，以更新所述地图模型。

10.根据权利要求8所述的方法，其特征在于，如果所述位姿参数是基于匹配成功的特征点及其对应的局部地图点而确定的，则所述根据所述新的关键帧以及所述位姿参数更新所述地图模型，包括：

获取所述新的关键帧以及与所述新的关键帧关联的其他关键帧，形成关键帧集合；

获取所述关键帧集合中出现过的全部地图点，形成地图点集合；

基于所述关键帧集合与其中每个关键帧的位姿参数、以及所述地图点集合，构建第三误差函数，所述第三误差函数包括重投影误差项，所述重投影误差项为所述地图点集合中的任一地图点到所述关键帧集合中的任一关键帧的重投影误差之和；

通过计算所述第三误差函数的最小值，优化所述关键帧集合中各关键帧的位姿参数以及所述地图点集合中各地图点的坐标，以更新所述地图模型。

11.根据权利要求10所述的方法，其特征在于，所述第三误差函数还包括帧间惯性测量误差项，为惯性测量单元在所述关键帧集合中任意两相邻关键帧之间的误差之和。

12.根据权利要求10所述的方法，其特征在于，所述与所述新的关键帧关联的其他关键帧包括：距离所述新的关键帧最近的M个关键帧，以及所述新的关键帧的N个共视关键帧；其中，M和N是预设的正整数。

13.根据权利要求10所述的方法，其特征在于，在更新所述地图模型时，还将所述地图点集合中满足第二预设条件的地图点从所述地图模型中删除；其中，所述第二预设条件包括：

所述地图点在所述关键帧集合中被跟踪成功的关键帧数量小于预测数量乘以预设比例，所述预设比例小于或等于1；或者

所述地图点在所述关键帧集合中各关键帧上的重投影误差的均值大于预设误差阈值。

14.根据权利要求10所述的方法，其特征在于，在更新所述地图模型时，若所述新的关键帧中存在与所述局部地图点不匹配的特征点，则将该特征点与所述关键帧集合中其他关键帧的特征点进行匹配，根据匹配的结果进行三角化计算，得到新的地图点，以添加到所述地图模型中。

15.根据权利要求8所述的方法，其特征在于，所述方法还包括：

针对所述新的关键帧进行回环检测，以对所述地图模型进行全局优化。

16.一种基于深度信息的位姿确定装置，其特征在于，所述装置包括：

图像获取模块，用于通过相机获取关于场景的当前帧图像，并获取所述当前帧图像的深度信息；

特征点提取模块，用于从所述当前帧图像提取包含深度信息的特征点；

特征点对齐模块，用于将所述特征点与所述场景的地图模型对齐，以确定所述相机在当前帧的位姿参数；

其中，所述将所述特征点与所述场景的地图模型对齐，以确定所述相机在当前帧的位姿参数，包括：

预测所述特征点在上一帧图像中的位置；根据所述地图模型对所述上一帧图像进行修正；基于修正后的所述上一帧图像、所述特征点在所述当前帧图像中的位置和在所述上一帧图像中的位置，构建第一误差函数；通过计算所述第一误差函数的最小值，确定所述相机在当前帧的位姿参数；及/或

根据所述特征点生成所述当前帧的三维点云；通过迭代最近邻点算法将所述地图模型中的局部模型与所述三维点云进行迭代匹配，并通过在所述地图模型中移动所述局部模型，以迭代优化所述三维点云与所述局部模型的匹配误差；将匹配误差最小时的位姿参数作为所述相机在当前帧的位姿参数。

17.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1-15任一项所述的方法。

18.一种电子设备，其特征在于，包括：

处理器；以及

存储器，用于存储所述处理器的可执行指令；

其中，所述处理器配置为经由执行所述可执行指令来执行权利要求1-15任一项所述的方法。

Images