CN108335327B - 摄像机姿态估计方法和摄像机姿态估计装置 - Google Patents

Abstract

公开了一种摄像机姿态估计方法和摄像机姿态估计装置，摄像机姿态估计方法包括：获得彩色图像和深度图像；获取当前帧彩色图像的至少一个特征点和相邻帧彩色图像的至少一个相应匹配特征点；获取与当前帧彩色图像中的每个特征点的位置分别对应的当前帧深度图像中的至少一个特征点，获取与当前帧深度图像的每个特征点分别匹配的相邻帧深度图像中的至少一个相应匹配特征点；在世界坐标系下计算当前帧深度图像中的每个特征点和相邻帧深度图像中的相应匹配特征点之间的距离，并且通过使得针对每对匹配的特征点所计算出的距离之和最小来估计当前摄像机姿态。根据本公开的实施例，可以利用彩色图像和深度图像信息来实现精确的摄像机参数估计。

Description

摄像机姿态估计方法和摄像机姿态估计装置

技术领域

本公开涉及摄像机的跟踪和定位领域，具体涉及一种同时利用彩色图像和深度图像信息来实现精确的摄像机参数估计的摄像机姿态估计方法和摄像机姿态估计装置。

背景技术

摄像机的跟踪及定位有着非常重要的应用，比如增强现实、三维建模以及机器人导航。例如，对于机器人来说，它需要知道自己在真实世界中的实时位置才能实现导航。尽管这个问题已经被研究了很多年，但是如何提高摄像机姿态估计的准确性和鲁棒性，仍然是一个非常有挑战的问题。

传统的实现摄像机姿态估计的方法可以分为两大类：基于视觉的方法和基于深度的方法。在基于视觉的方法中，主要通过建立稀疏匹配点之间的几何关系来求解摄像机参数。这种方法的缺点在于，参数估计对于彩色图像的质量非常敏感。当没有足够丰富的匹配点时，结果会变得很差。基于深度的方法只利用稠密的深度信息来估计摄像机的姿态，常用的算法为迭代的最近点算法。但是这种方法对参数初始值的精度要求较高，估计较大角度姿态的能力不足。

发明内容

在下文中给出了关于本公开的简要概述，以便提供关于本公开的某些方面的基本理解。但是，应当理解，这个概述并不是关于本公开的穷举性概述。它并不是意图用来确定本公开的关键性部分或重要部分，也不是意图用来限定本公开的范围。其目的仅仅是以简化的形式给出关于本公开的某些概念，以此作为稍后给出的更详细描述的前序。

鉴于以上问题，本公开的目的是提供一种同时利用彩色图像和深度图像信息来实现精确的摄像机参数估计的摄像机姿态估计方法和摄像机姿态估计装置。

根据本公开的一方面，提供了一种摄像机姿态估计方法，包括：通过摄像机获得彩色图像和与彩色图像对应的深度图像；获取当前帧彩色图像中的至少一个特征点和与所述至少一个特征点中的每个分别匹配的相邻帧彩色图像中的至少一个相应匹配特征点；获取与当前帧彩色图像中的所述至少一个特征点中的每个的位置分别对应的当前帧深度图像中的至少一个特征点，并且基于相邻帧彩色图像中的所述至少一个相应匹配特征点，获取与当前帧深度图像中的所述至少一个特征点中的每个分别匹配的相邻帧深度图像中的至少一个相应匹配特征点；以及在世界坐标系下计算所述当前帧深度图像中的每个特征点和所述相邻帧深度图像中的相应匹配特征点之间的距离，并且通过使得针对每对匹配的特征点所计算出的距离之和最小来估计当前摄像机姿态。

根据本公开的另一方面，提供了一种摄像机姿态估计装置，包括：彩色图像和深度图像获取单元，被配置成通过摄像机获得彩色图像和与彩色图像对应的深度图像；彩色图像特征点获取单元，被配置成获取当前帧彩色图像中的至少一个特征点和与所述至少一个特征点中的每个分别匹配的相邻帧彩色图像中的至少一个相应匹配特征点；深度图像特征点获取单元，被配置成获取与当前帧彩色图像中的所述至少一个特征点中的每个的位置分别对应的当前帧深度图像中的至少一个特征点，并且基于相邻帧彩色图像中的所述至少一个相应匹配特征点，获取与当前帧深度图像中的所述至少一个特征点中的每个分别匹配的相邻帧深度图像中的至少一个相应匹配特征点；以及摄像机姿态估计单元，被配置成在世界坐标系下计算所述当前帧深度图像中的每个特征点和所述相邻帧深度图像中的相应匹配特征点之间的距离，并且通过使得针对每对匹配的特征点所计算出的距离之和最小来估计当前摄像机姿态。

根据本公开的其它方面，还提供了用于实现上述根据本公开的方法的计算机程序代码和计算机程序产品以及其上记录有该用于实现上述根据本公开的方法的计算机程序代码的计算机可读存储介质。

在下面的说明书部分中给出本公开实施例的其它方面，其中，详细说明用于充分地公开本公开实施例的优选实施例，而不对其施加限定。

附图说明

本公开可以通过参考下文中结合附图所给出的详细描述而得到更好的理解，其中在所有附图中使用了相同或相似的附图标记来表示相同或者相似的部件。所述附图连同下面的详细说明一起包含在本说明书中并形成说明书的一部分，用来进一步举例说明本公开的优选实施例和解释本公开的原理和优点。其中：

图1示出示出根据本公开的实施例的摄像机姿态估计方法的流程示例的流程图；

图2是示出根据本公开实施例的深度图像中的特征点的三维空间投影的示意图；

图3是示出根据本公开实施例的组中的深度图像中的特征点的三维空间投影的示意图；

图4是示出根据本公开的实施例的摄像机姿态估计装置的功能配置示例的框图；以及

图5是示出作为本公开的实施例中可采用的信息处理装置的个人计算机的示例结构的框图。

具体实施方式

在下文中将结合附图对本公开的示范性实施例进行描述。为了清楚和简明起见，在说明书中并未描述实际实施方式的所有特征。然而，应该了解，在开发任何这种实际实施例的过程中必须做出很多特定于实施方式的决定，以便实现开发人员的具体目标，例如，符合与系统及业务相关的那些限制条件，并且这些限制条件可能会随着实施方式的不同而有所改变。此外，还应该了解，虽然开发工作有可能是非常复杂和费时的，但对得益于本公开内容的本领域技术人员来说，这种开发工作仅仅是例行的任务。

在此，还需要说明的一点是，为了避免因不必要的细节而模糊了本公开，在附图中仅仅示出了与根据本公开的方案密切相关的设备结构和/或处理步骤，而省略了与本公开关系不大的其它细节。

根据本公开的一方面，提出了一种摄像机姿态估计方法，该摄像机姿态估计方法同时利用彩色图像和深度图像信息，彩色图像序列用来获取稀疏的匹配点，深度图像序列用来获取这些匹配点的深度值，通过在三维空间中建立关于这些匹配点的几何约束，实现精确的摄像机参数估计。

下面结合附图详细说明根据本公开的实施例。

首先，将参照图1描述根据本公开实施例的摄像机姿态估计方法100的流程示例。图1是示出根据本公开的实施例的摄像机姿态估计方法100的流程示例的流程图。如图1所示，根据本公开的实施例的摄像机姿态估计方法100包括彩色图像和深度图像获取步骤S102、彩色图像特征点获取步骤S104、深度图像特征点获取步骤S106以及摄像机姿态估计步骤S108。

在彩色图像和深度图像获取步骤S102中，可以通过摄像机获得彩色图像和与彩色图像对应的深度图像。这样，可以分别获得彩色图像序列以及包括与彩色图像序列中的每个彩色图像分别对应的深度图像的深度图像序列。

在彩色图像特征点获取步骤S104中，可以获取当前帧彩色图像中的至少一个特征点和与所述至少一个特征点中的每个分别匹配的相邻帧彩色图像中的至少一个相应匹配特征点。

优选地，当前帧彩色图像的所述至少一个特征点和相邻帧彩色图像的所述至少一个相应匹配特征点是通过如下方式获取的：在当前帧彩色图像中检测所述至少一个特征点，并且针对所检测到的所述至少一个特征点中的每个特征点，在相邻帧彩色图像中查找其相应匹配特征点，从而可以获取相邻帧彩色图像中的所述至少一个相应匹配特征点。

具体地，从当前帧彩色图像中进行特征点的检测，从而可以获取当前帧彩色图像中的至少一个特征点。常用的检测方法包括SIFT(尺度不变特征变换)算法、SURF(加速鲁棒特征)算法以及KLT(Kanade-Lucas-Tomasi)算法等。然后在相邻的两帧彩色图像之间进行特征点的匹配，即，针对当前帧彩色图像的所检测到的所述至少一个特征点中的每个特征点，在相邻帧彩色图像中查找其相应匹配特征点，从而获取相邻帧彩色图像中的至少一个相应匹配特征点。每对匹配的特征点随着时间反投影为一个特征跟踪轨迹，特征跟踪轨迹中的每个点对应三维空间中的一个点。令

表示第i个轨迹，其表示第i对匹配的特征点随着时间反投影所形成的一个特征跟踪轨迹，其中，i＝0,1,…,N-1，N(N≥1)是相邻两个彩色图像之间的匹配的特征点对的总数量；k为图像的序号；

为第k帧彩色图像上与轨迹χ_i对应的一个点(具体地，

表示第k帧彩色图像上的第i个特征点，

可以表示为矢量形式)，f(χ_i)表示轨迹χ_i经过的所有图像帧的集合。这些特征跟踪轨迹代表稀疏的对应关系，将用来估计摄像机的参数。

优选地，在获取相邻帧彩色图像的匹配特征点时，只有在相邻帧彩色图像中的匹配特征点存在于当前帧彩色图像之前的预定数量的帧中的情况下，才将该匹配特征点确定为最终的匹配特征点，以提高特征点匹配的精度。

对于上述轨迹χ_i，可以设置一个最小的轨迹长度来去除错误的匹配点。具体地，只有轨迹χ_i的长度为预定数量的情况下，才将该匹配特征点确定为最终的匹配特征点，以提高特征点匹配的精度。其中，可以根据经验确定上述预定数量。

在深度图像特征点获取步骤S106中，可以获取与当前帧彩色图像中的所述至少一个特征点中的每个的位置分别对应的当前帧深度图像中的至少一个特征点，并且可以基于相邻帧彩色图像中的所述至少一个相应匹配特征点，获取与当前帧深度图像中的所述至少一个特征点中的每个分别匹配的相邻帧深度图像中的至少一个相应匹配特征点。

优选地，当前帧深度图像的所述至少一个特征点和相邻帧深度图像的所述至少一个相应匹配特征点是通过如下方式获取的：可以将当前帧彩色图像中的所述至少一个特征点映射到当前帧深度图像中，以获取当前帧深度图像中的所述至少一个特征点；以及可以将相邻帧彩色图像中的所述至少一个特征点映射到相邻帧深度图像中，以获取相邻帧深度图像中的所述至少一个特征点。

具体地，可以将当前帧彩色图像中某个位置处的特征点映射到当前帧深度图像中的对应位置处，从而获取与当前帧彩色图像中的该特征点对应的当前帧深度图像中的一个特征点，并且可以依次将当前帧彩色图像中的每个特征点映射到当前帧深度图像中的对应位置处，从而获取与当前帧彩色图像中的每个特征点分别对应的当前帧深度图像中的特征点；可以将相邻帧彩色图像中某个位置处的特征点映射到相邻帧深度图像中的对应位置处，从而获取与相邻帧彩色图像中的该特征点对应的相邻帧深度图像中的一个特征点，并且可以依次将相邻帧彩色图像中的每个特征点映射到相邻帧深度图像中的对应位置处，从而获取与相邻帧彩色图像中的每个特征点分别对应的相邻帧深度图像中的特征点。

在摄像机姿态估计步骤S108中，可以在世界坐标系下计算当前帧深度图像中的每个特征点和相邻帧深度图像中的相应匹配特征点之间的距离，并且可以通过使得针对每对匹配的特征点所计算出的距离之和最小来估计当前摄像机姿态。

在一个例子中，摄像机姿态包含三个旋转参数以及三个平移参数，假设采用无径向畸变和无切向畸变的针孔摄像机模型，可以将第k帧图像对应的摄像机姿态表示为一个刚性变换矩阵的形式：

其中R为3*3的旋转矩阵，t为3*1的平移向量。变换矩阵T_g,k可以将第k帧的摄像机坐标转化为世界坐标，比如在摄像机坐标下的三维点X_c可以通过公式

转为世界坐标下的三维点X_g。其中

表示齐次向量：

为了实现摄像机姿态的估计，需要建立匹配的特征点之间的几何对应关系。首先，假设第k帧彩色图像上的一个特征点为

根据与第k帧彩色图像对应的第k帧深度图像，可获取该特征点

在第k帧深度图像中的相同位置处的对应特征点及其深度值

同时根据摄像机校正矩阵K，可将第k帧深度图像中的对应特征点反投影到当前摄像机坐标下的三维点X_c，k(u)，公式如下：

其中，摄像机校正矩阵K通常是常值，表达式如下：

在上式中，f为焦距，(c_x，c_y)为主点坐标。K的作用为将摄像机坐标转化为图像平面坐标。

上文中以彩色图像为例介绍了χ_i。本领域技术人员可以容易理解，因为彩色图像序列中的每个彩色图像分别与深度图像序列中的每个深度图像对应，因此深度图像中的每对匹配的特征点随着时间也反投影为一个特征跟踪轨迹，特征跟踪轨迹中的每个点为三维空间中的一个点。除非特别说明，在以下描述中，χ_i指的是深度图像中的每对匹配的特征点随着时间反投影所形成的特征跟踪轨迹。

在世界坐标系下，第k帧深度图像上与特征跟踪轨迹χ_i对应的特征点的三维坐标等价于第k帧深度图像上的该特征点处的三维坐标，则第k帧深度图像上与特征跟踪轨迹χ_i对应的特征点(即，第k帧深度图像上的第i个特征点)在世界坐标系下的三维坐标可通过摄像机姿态T_g,k计算得到：

优选地，可以基于当前帧深度图像中的每个特征点的世界坐标和相邻帧深度图像中的相应匹配特征点的世界坐标，计算该特征点与其相应匹配特征点之间的距离。即，在世界坐标系下，计算当前深度图像中的每个特征点和相邻帧深度图像中的相应匹配特征点之间的距离。

以下为了方便描述，假设当前帧深度图像为第k帧深度图像，并假设第k帧深度图像的相邻帧深度图像为第k-1帧深度图像。

图2是示出根据本公开实施例的深度图像中的特征点的三维空间投影的示意图。图2中右侧的图像B上的圆点b1和b2为第k帧深度图像中的两个特征点，图2中左侧的图像A上的圆点a1和a2分别为在作为第k帧深度图像的相邻帧深度图像的第k-1帧深度图像上与特征点b1和b2分别匹配的特征点。在理想情况下，相邻两个帧上的一对匹配特征点在世界坐标系中反投影为三维空间中的同一个点，例如，如图2所示，第k帧深度图像中的特征点b1和第k-1帧深度图像中的相应匹配特征点a1在世界坐标系中反投影为三维空间中的同一个点c1，并且第k帧深度图像中的特征点b2和第k-1帧深度图像中的相应匹配特征点a2在世界坐标系中反投影为三维空间中的同一个点c2。相反，图2还示出了第k帧深度图像中的特征点b2在世界坐标系中反投影为三维空间中的点c2’的情况，在这种情况下，第k帧深度图像中的特征点b2反投影形成的三维空间中的点c2’不同于第k-1帧深度图像的相应匹配特征点a2反投影形成的三维空间中的点c2，为了使得该匹配特征点对反投影为三维空间中的同一个点，应该使图2中示出的圆圈内的两个点c2和c2’在世界坐标系中的距离最小。根据这个原则我们可以通过最小化第k帧深度图像上的每个特征点与已求解的第k-1帧深度图像上的匹配特征点的世界坐标系下的距离之和来估计当前摄像机的姿态。即，通过使得在世界坐标系下的、针对当前帧深度图像(第k帧深度图像)的特征点和相邻帧深度图像(第k-1帧深度图像)的每对匹配特征点所计算出的距离之和最小，估计第k帧的摄像机姿态。具体公式如下：

在公式(4)中，E()表示误差函数，

为与第k帧彩色图像中的第i个特征点

对应的第k帧深度图像中的第i个特征点在摄像机坐标下的三维坐标，可以由公式(2)求出，

表示与第k帧深度图像中的第i个特征点匹配的第k-1帧深度图像中的第i个特征点的世界坐标，其中，i＝0,1,…,N-1，N为第k帧深度图像和第k-1帧深度图像中的匹配的特征点对的总数量。通过最小化公式(4)，即可得到第k帧图像对应的摄像机的参数T_g,k。

在以上描述中，示例而非限制，将第k-1帧图像作为第k帧图像的相邻帧。此外，还可以将第k帧图像之前的任意帧作为第k帧图像的相邻帧。

优选地，对于一个图像序列，将第一帧深度图像的摄像机坐标系设置为世界坐标系；以及摄像机姿态为用于将摄像机坐标系变换到世界坐标系的变换矩阵。

由公式(3)可以看出，摄像机姿态为用于将摄像机坐标系变换到世界坐标系的变换矩阵。在对一个图像序列进行处理时，设置第一帧(假设k＝1)深度图像的摄像机坐标系为世界坐标系，即将第一帧深度图像的摄像机姿态T_g,1设置为单位矩阵。那么，第一帧上的特征点在世界坐标下的三维点坐标可由公式(2)和(3)得到。对于第k帧图像，根据该帧上的特征点所在的特征轨迹，选取出那些已经被求解出三维点坐标

的第k-1帧上的特征点，其中，i＝0,1,…,N-1。对于第k帧深度图像中的每个特征点，可以根据公式(2)得到该特征点在摄像机坐标下的三维点坐标

然后可以根据公式(3)得到该特征点在世界坐标下的三维点坐标

利用公式(4)，在世界坐标系下计算第k-1帧深度图像中的每个特征点和第k-1帧深度图像中相应匹配特征点之间的距离，并且通过使得针对每对匹配的特征点所计算出的距离之和最小，可以估计出第k帧对应的摄像机姿态。

优选地，在估计出当前摄像机姿态的情况下，基于所估计的当前摄像机姿态来计算当前帧深度图像中的所有特征点的世界坐标；以及对于当前帧深度图像中的每个特征点：如果该特征点也存在于当前帧深度图像之前的深度图像中，则将该特征点在当前帧深度图像中的世界坐标与该特征点在当前帧深度图像之前的深度图像中的世界坐标进行平均，作为该特征点在当前帧深度图像中的更新后的世界坐标。

具体地，在估计出第k帧图像所对应的摄像机姿态T_g,k的情况下，基于所估计的摄像机姿态T_g,k来计算第k帧深度图像中的所有特征点的世界坐标；以及对于第k帧深度图像中的每个特征点：如果该特征点也存在于第k帧深度图像之前的深度图像中，则将该特征点在第k帧深度图像中的世界坐标与该特征点在第k帧深度图像之前的深度图像中的世界坐标进行平均，作为该特征点在第k帧深度图像中的更新后的世界坐标。换句话说，如果某个特征点所在的特征跟踪轨迹在第k帧之前已经求解出世界坐标系下的三维点坐标，则将它们取平均作为对应轨迹的新的世界坐标系下的三维点坐标。

在对一个图像序列进行处理时，这种渐进式的摄像机姿态求解方式容易造成时序上的误差累积，从而导致摄像机姿态的估计严重偏离正确的取值。为此，可以采取全局优化的方式来降低累积误差。全局优化即同时优化多帧的摄像机姿态。

优选地，根据本公开实施例的摄像机姿态估计方法还可以包括：将深度图像的序列划分成组；以及对于每个组，基于与该组中的每帧深度图像的所有特征点和该组中的所有其他帧深度图像的所有相应匹配特征点有关的组几何约束，优化该组中的每帧深度图像对应的摄像机姿态。

优选地，所述组几何约束为：使得在世界坐标系下的、该组中的所有对匹配特征点之间的距离之和最小。

图3是示出根据本公开实施例的组中的深度图像中的特征点的三维空间投影的示意图。图3中图像C上的圆点c1为第k帧深度图像中的一个特征点，图像B上的圆点b1为在作为第k帧深度图像的相邻帧深度图像的第k-1帧深度图像上与特征点c1匹配的特征点，以及图像A上的圆点a1为在作为第k-1帧深度图像的相邻帧深度图像的第k-2帧深度图像上与特征点b1匹配的特征点。如图3所示，特征点a1在世界坐标系中反投影为点d1、特征点b1在世界坐标系中反投影为点d1’以及特征点c1在世界坐标系中反投影为点d1”。为了使得相互匹配的特征点反投影为三维空间中的同一个点，应该使得在世界坐标系中的、点d1与d1’之间的距离以及d1’与d1”之间的距离之和最小。根据这个原则我们可以通过使组中的所有对匹配特征点之间的距离之和最小来优化该组中的每帧深度图像对应的摄像机的姿态。

具体地，下面以将第1帧至第k帧深度图像划分成一组为例，介绍对该组进行全局优化。对于该组，全局优化的损失函数可以表示为：

在公式(5)中，E()为误差函数；

和

表示在第m帧彩色图像和第n帧彩色图像中的第i对匹配特征点，

和

在同一条轨迹χ_i上；i＝0,1,…,M-1，M(M≥1)是该组中匹配的特征点对的总数量；

和

分别是

和

在第m帧深度图像和第n帧深度图像中的对应特征点反投影到摄像机坐标下的三维点；T_g,k、T_g,p、T_g,1、T_g,m以及T_g,n分别是第k帧、第p帧、第1帧、第m帧以及第n帧对应的摄像机姿态。最小化公式(5)可以使得包括第1帧至第k帧深度图像的组中所有匹配特征点对应的世界坐标系下的三维点坐标尽可能地相近，从而降低了误差的累积，进一步提高了摄像机参数估计的精度。全局优化可以在任意时刻进行，但该算法复杂度较高，每帧都进行全局优化是不必要的。可以通过监控当前帧上的误差值，当误差值超过某个阈值时，才采用全局优化的策略。

综上所述，根据本公开的实施例的摄像机姿态估计方法100同时利用了彩色图像和深度图像信息，彩色图像序列用来获取稀疏的匹配点，深度图像信息序列用来获取这些匹配点的深度值，同时在三维空间中建立关于这些匹配点的几何约束，从而实现摄像机参数的精确估计。

与上述摄像机姿态估计方法实施例相对应地，本公开还提供了以下摄像机姿态估计装置的实施例。

图4是示出根据本公开的实施例的摄像机姿态估计装置400的功能配置示例的框图。

如图4所示，根据本公开的实施例的摄像机姿态估计装置400可以包括彩色图像和深度图像获取单元402、彩色图像特征点获取单元404、深度图像特征点获取单元406以及摄像机姿态估计单元408。接下来将描述各个单元的功能配置示例。

在彩色图像和深度图像获取单元402中，可以通过摄像机获得彩色图像和与彩色图像对应的深度图像。这样，可以分别获得彩色图像序列以及包括与彩色图像序列中的每个彩色图像分别对应的深度图像的深度图像序列。

在彩色图像特征点获取单元404中，可以获取当前帧彩色图像中的至少一个特征点和与所述至少一个特征点中的每个分别匹配的相邻帧彩色图像中的至少一个相应匹配特征点。

优选地，当前帧彩色图像的所述至少一个特征点和相邻帧彩色图像的所述至少一个相应匹配特征点是通过如下方式获取的：在当前帧彩色图像中检测所述至少一个特征点，并且针对所检测到的所述至少一个特征点中的每个特征点，在相邻帧彩色图像中查找其相应匹配特征点，从而可以获取相邻帧彩色图像中的所述至少一个相应匹配特征点。

获取当前帧彩色图像的所述至少一个特征点和相邻帧彩色图像的所述至少一个相应匹配特征点的示例可以参见以上方法实施例中相应位置的描述，在此不再重复。

每对匹配的特征点随着时间反投影为一个特征跟踪轨迹，特征跟踪轨迹中的每个点为三维空间中的一个点。

表示第i个轨迹，其表示第i对匹配的特征点随着时间反投影所形成的一个特征跟踪轨迹。有关参数的具体说明可以参见以上方法实施例中相应位置的描述，在此不再重复。这些特征跟踪轨迹代表稀疏的对应关系，将用来估计摄像机的参数。

优选地，在获取相邻帧彩色图像的匹配特征点时，只有在相邻帧彩色图像中的匹配特征点存在于当前帧彩色图像之前的预定数量的帧中的情况下，才将该匹配特征点确定为最终的匹配特征点，以提高特征点匹配的精度。

对于上述轨迹χ_i，可以设置一个最小的轨迹长度来去除错误的匹配点。具体地，只有轨迹χ_i的长度为预定数量的情况下，才将该匹配特征点确定为最终的匹配特征点，以提高特征点匹配的精度。其中，可以根据经验确定上述预定数量。

在深度图像特征点获取单元406中，可以获取与当前帧彩色图像中的所述至少一个特征点中的每个的位置分别对应的当前帧深度图像中的至少一个特征点，并且可以基于相邻帧彩色图像中的所述至少一个相应匹配特征点，获取与当前帧深度图像中的所述至少一个特征点中的每个分别匹配的相邻帧深度图像中的至少一个相应匹配特征点。

优选地，当前帧深度图像的所述至少一个特征点和相邻帧深度图像的所述至少一个相应匹配特征点是通过如下方式获取的：可以将当前帧彩色图像中的所述至少一个特征点映射到当前帧深度图像中，以获取当前帧深度图像中的所述至少一个特征点；以及可以将相邻帧彩色图像中的所述至少一个特征点映射到相邻帧深度图像中，以获取相邻帧深度图像中的所述至少一个特征点。

获取当前帧深度图像的所述至少一个特征点和相邻帧深度图像的所述至少一个相应匹配特征点的示例可以参见以上方法实施例中相应位置的描述，在此不再重复。

在摄像机姿态估计单元408中，可以在世界坐标系下计算当前帧深度图像中的每个特征点和相邻帧深度图像中的相应匹配特征点之间的距离，并且可以通过使得针对每对匹配的特征点所计算出的距离之和最小来估计当前摄像机姿态。

优选地，可以基于当前帧深度图像中的每个特征点的世界坐标和相邻帧深度图像中的相应匹配特征点的世界坐标，计算该特征点与其相应匹配特征点之间的距离。即，在世界坐标系下，计算当前深度图像中的每个特征点和相邻帧深度图像中的相应匹配特征点之间的距离。

计算当前深度图像中的每个特征点的世界坐标和相邻帧深度图像中的相应匹配特征点之间的距离以及估计当前摄像机姿的示例可以参见以上方法实施例中相应位置的描述，在此不再重复。

优选地，对于一个图像序列，将第一帧深度图像的摄像机坐标系设置为世界坐标系；以及摄像机姿态为用于将摄像机坐标系变换到世界坐标系的变换矩阵。具体示例可以参见以上方法实施例中相应位置的描述，在此不再重复。

优选地，在估计出当前摄像机姿态的情况下，基于所估计的当前摄像机姿态来计算当前帧深度图像中的所有特征点的世界坐标；以及对于当前帧深度图像中的每个特征点：如果该特征点也存在于当前帧深度图像之前的深度图像中，则将该特征点在当前帧深度图像中的世界坐标与该特征点在当前帧深度图像之前的深度图像中的世界坐标进行平均，作为该特征点在当前帧深度图像中的更新后的世界坐标。

具体地，在估计出第k帧图像所对应的摄像机姿态的情况下，基于所估计的摄像机姿态来计算第k帧深度图像中的所有特征点的世界坐标；以及对于第k帧深度图像中的每个特征点：如果该特征点也存在于第k帧深度图像之前的深度图像中，则将该特征点在第k帧深度图像中的世界坐标与该特征点在第k帧深度图像之前的深度图像中的世界坐标进行平均，作为该特征点在第k帧深度图像中的更新后的世界坐标。换句话说，如果某个特征点所在的特征跟踪轨迹在第k帧之前已经求解出世界坐标系下的三维点坐标，则将它们取平均作为对应轨迹的新的世界坐标系下的三维点坐标。

在对一个图像序列进行处理时，这种渐进式的摄像机姿态求解方式容易造成时序上的误差累积，从而导致摄像机姿态的估计严重偏离正确的取值。为此，可以采取全局优化的方式来降低累积误差。全局优化即同时优化多帧的摄像机姿态。

优选地，根据本公开实施例的摄像机姿态估计装置还可以包括优化单元，其可以被配置成：将深度图像的序列划分成组；以及对于每个组，基于与该组中的每帧深度图像的所有特征点和该组中的所有其他帧深度图像的所有相应匹配特征点有关的组几何约束，优化该组中的每帧深度图像对应的摄像机姿态。

优选地，所述组几何约束为：使得在世界坐标系下的、该组中的所有对匹配特征点之间的距离之和最小。

全局优化的示例可以参见以上方法实施例中相应位置的描述，在此不再重复。

全局优化可以在任意时刻进行，但该算法复杂度较高，每帧都进行全局优化是不必要的。可以通过监控当前帧上的误差值，当误差值超过某个阈值时，才采用全局优化的策略。

综上所述，根据本公开的实施例的摄像机姿态估计装置400同时利用了彩色图像和深度图像信息，彩色图像序列用来获取稀疏的匹配点，深度图像信息序列用来获取这些匹配点的深度值，同时在三维空间中建立关于这些匹配点的几何约束，从而实现摄像机参数的精确估计。

应指出，尽管以上描述了根据本公开的实施例的摄像机姿态估计装置的功能配置，但是这仅是示例而非限制，并且本领域技术人员可根据本公开的原理对以上实施例进行修改，例如可对各个实施例中的功能模块进行添加、删除或者组合等，并且这样的修改均落入本公开的范围内。

此外，还应指出，这里的装置实施例是与上述方法实施例相对应的，因此在装置实施例中未详细描述的内容可参见方法实施例中相应位置的描述，在此不再重复描述。

应理解，根据本公开的实施例的存储介质和程序产品中的机器可执行的指令还可以被配置成执行上述摄像机姿态估计方法，因此在此未详细描述的内容可参考先前相应位置的描述，在此不再重复进行描述。

相应地，用于承载上述包括机器可执行的指令的程序产品的存储介质也包括在本发明的公开中。该存储介质包括但不限于软盘、光盘、磁光盘、存储卡、存储棒等等。

另外，还应该指出的是，上述系列处理和装置也可以通过软件和/或固件实现。在通过软件和/或固件实现的情况下，从存储介质或网络向具有专用硬件结构的计算机，例如图5所示的通用个人计算机500安装构成该软件的程序，该计算机在安装有各种程序时，能够执行各种功能等等。

在图5中，中央处理单元(CPU)501根据只读存储器(ROM)502中存储的程序或从存储部分508加载到随机存取存储器(RAM)503的程序执行各种处理。在RAM 503中，也根据需要存储当CPU 501执行各种处理等时所需的数据。

CPU 501、ROM 502和RAM 503经由总线504彼此连接。输入/输出接口505也连接到总线504。

下述部件连接到输入/输出接口505：输入部分506，包括键盘、鼠标等；输出部分507，包括显示器，比如阴极射线管(CRT)、液晶显示器(LCD)等，和扬声器等；存储部分508，包括硬盘等；和通信部分509，包括网络接口卡比如LAN卡、调制解调器等。通信部分509经由网络比如因特网执行通信处理。

根据需要，驱动器510也连接到输入/输出接口505。可拆卸介质511比如磁盘、光盘、磁光盘、半导体存储器等等根据需要被安装在驱动器510上，使得从中读出的计算机程序根据需要被安装到存储部分508中。

在通过软件实现上述系列处理的情况下，从网络比如因特网或存储介质比如可拆卸介质511安装构成软件的程序。

本领域的技术人员应当理解，这种存储介质不局限于图5所示的其中存储有程序、与设备相分离地分发以向用户提供程序的可拆卸介质511。可拆卸介质511的例子包含磁盘(包含软盘(注册商标))、光盘(包含光盘只读存储器(CD-ROM)和数字通用盘(DVD))、磁光盘(包含迷你盘(MD)(注册商标))和半导体存储器。或者，存储介质可以是ROM 502、存储部分508中包含的硬盘等等，其中存有程序，并且与包含它们的设备一起被分发给用户。

以上参照附图描述了本公开的优选实施例，但是本公开当然不限于以上示例。本领域技术人员可在所附权利要求的范围内得到各种变更和修改，并且应理解这些变更和修改自然将落入本公开的技术范围内。

例如，在以上实施例中包括在一个单元中的多个功能可以由分开的装置来实现。替选地，在以上实施例中由多个单元实现的多个功能可分别由分开的装置来实现。另外，以上功能之一可由多个单元来实现。无需说，这样的配置包括在本公开的技术范围内。

在该说明书中，流程图中所描述的步骤不仅包括以所述顺序按时间序列执行的处理，而且包括并行地或单独地而不是必须按时间序列执行的处理。此外，甚至在按时间序列处理的步骤中，无需说，也可以适当地改变该顺序。

另外，根据本公开的技术还可以如下进行配置。

附记1.一种摄像机姿态估计方法，包括：

通过摄像机获得彩色图像和与所述彩色图像对应的深度图像；

获取当前帧彩色图像中的至少一个特征点和与所述至少一个特征点中的每个分别匹配的相邻帧彩色图像中的至少一个相应匹配特征点；

获取与所述当前帧彩色图像中的所述至少一个特征点中的每个的位置分别对应的当前帧深度图像中的至少一个特征点，并且基于所述相邻帧彩色图像中的所述至少一个相应匹配特征点，获取与所述当前帧深度图像中的所述至少一个特征点中的每个分别匹配的相邻帧深度图像中的至少一个相应匹配特征点；以及

在世界坐标系下计算所述当前帧深度图像中的每个特征点和所述相邻帧深度图像中的相应匹配特征点之间的距离，并且通过使得针对每对匹配的特征点所计算出的距离之和最小来估计当前摄像机姿态。

附记2.根据附记1所述的摄像机姿态估计方法，其中，

将第一帧深度图像的摄像机坐标系设置为世界坐标系；以及

摄像机姿态为用于将摄像机坐标系变换到世界坐标系的变换矩阵。

附记3.根据附记1所述的摄像机姿态估计方法，其中，基于所述当前帧深度图像中的每个特征点的世界坐标和所述相邻帧深度图像中的相应匹配特征点的世界坐标，计算该特征点与其相应匹配特征点之间的距离。

附记4.根据附记3所述的摄像机姿态估计方法，

其中，在估计出所述当前摄像机姿态的情况下，基于所估计的当前摄像机姿态来计算所述当前帧深度图像中的所有特征点的世界坐标；以及

对于所述当前帧深度图像中的每个特征点：

如果该特征点也存在于所述当前帧深度图像之前的深度图像中，则将该特征点在所述当前帧深度图像中的世界坐标与该特征点在所述当前帧深度图像之前的深度图像中的世界坐标进行平均，作为该特征点在所述当前帧深度图像中的更新后的世界坐标。

附记5.根据附记1所述的摄像机姿态估计方法，还包括：

将深度图像的序列划分成组；以及

对于每个组，基于与该组中的每帧深度图像的所有特征点和该组中的所有其他帧深度图像的所有相应匹配特征点有关的组几何约束，优化该组中的每帧深度图像对应的摄像机姿态。

附记6.根据附记5所述的摄像机姿态估计方法，其中，所述组几何约束为：

使得在世界坐标系下的、该组中的所有对匹配特征点之间的距离之和最小。

附记7.根据附记1所述的摄像机姿态估计方法，其中，在获取所述相邻帧彩色图像的匹配特征点时，只有在所述相邻帧彩色图像中的匹配特征点存在于所述当前帧彩色图像之前的预定数量的帧中的情况下，才将该匹配特征点确定为最终的匹配特征点，以提高特征点匹配的精度。

附记8.根据附记1所述的摄像机姿态估计方法，其中，所述当前帧彩色图像的所述至少一个特征点和所述相邻帧彩色图像的所述至少一个相应匹配特征点是通过如下方式获取的：在所述当前帧彩色图像中检测所述至少一个特征点，并且针对所检测到的所述至少一个特征点中的每个特征点，在所述相邻帧彩色图像中查找其相应匹配特征点，从而获取所述相邻帧彩色图像中的所述至少一个相应匹配特征点。

附记9.根据附记1所述的摄像机姿态估计方法，其中，所述当前帧深度图像的所述至少一个特征点和所述相邻帧深度图像的所述至少一个相应匹配特征点是通过如下方式获取的：

将所述当前帧彩色图像中的所述至少一个特征点映射到所述当前帧深度图像中，以获取所述当前帧深度图像中的所述至少一个特征点；以及

将所述相邻帧彩色图像中的所述至少一个特征点映射到所述相邻帧深度图像中，以获取所述相邻帧深度图像中的所述至少一个特征点。

附记10.一种摄像机姿态估计装置，包括：

彩色图像和深度图像获取单元，被配置成通过摄像机获得彩色图像和与所述彩色图像对应的深度图像；

彩色图像特征点获取单元，被配置成获取当前帧彩色图像中的至少一个特征点和与所述至少一个特征点中的每个分别匹配的相邻帧彩色图像中的至少一个相应匹配特征点；

深度图像特征点获取单元，被配置成获取与所述当前帧彩色图像中的所述至少一个特征点中的每个的位置分别对应的当前帧深度图像中的至少一个特征点，并且基于所述相邻帧彩色图像中的所述至少一个相应匹配特征点，获取与所述当前帧深度图像中的所述至少一个特征点中的每个分别匹配的相邻帧深度图像中的至少一个相应匹配特征点；以及

摄像机姿态估计单元，被配置成在世界坐标系下计算所述当前帧深度图像中的每个特征点和所述相邻帧深度图像中的相应匹配特征点之间的距离，并且通过使得针对每对匹配的特征点所计算出的距离之和最小来估计当前摄像机姿态。

附记11.根据附记10所述的摄像机姿态估计装置，其中，

将第一帧深度图像的摄像机坐标系设置为世界坐标系；以及

摄像机姿态为用于将摄像机坐标系变换到世界坐标系的变换矩阵。

附记12.根据附记10所述的摄像机姿态估计装置，其中，基于所述当前帧深度图像中的每个特征点的世界坐标和所述相邻帧深度图像中的相应匹配特征点的世界坐标，计算该特征点与其相应匹配特征点之间的距离。

附记13.根据附记12所述的摄像机姿态估计装置，

其中，在估计出所述当前摄像机姿态的情况下，基于所估计的当前摄像机姿态来计算所述当前帧深度图像中的所有特征点的世界坐标；以及

对于所述当前帧深度图像中的每个特征点：

如果该特征点也存在于所述当前帧深度图像之前的深度图像中，则将该特征点在所述当前帧深度图像中的世界坐标与该特征点在所述当前帧深度图像之前的深度图像中的世界坐标进行平均，作为该特征点在所述当前帧深度图像中的更新后的世界坐标。

附记14.根据附记10所述的摄像机姿态估计装置，还包括优化单元，所述优化单元被配置成：

将深度图像的序列划分成组；以及

对于每个组，基于与该组中的每帧深度图像的所有特征点和该组中的所有其他帧深度图像的所有相应匹配特征点有关的组几何约束，优化该组中的每帧深度图像对应的摄像机姿态。

附记15.根据附记14所述的摄像机姿态估计装置，其中，所述组几何约束为：

使得在世界坐标系下的、该组中的所有对匹配特征点之间的距离之和最小。

附记16.根据附记10所述的摄像机姿态估计装置，其中，在获取所述相邻帧彩色图像的匹配特征点时，只有在所述相邻帧彩色图像中的匹配特征点存在于所述当前帧彩色图像之前的预定数量的帧中的情况下，才将该匹配特征点确定为最终的匹配特征点，以提高特征点匹配的精度。

附记17.根据附记10所述的摄像机姿态估计装置，其中，所述当前帧彩色图像的所述至少一个特征点和所述相邻帧彩色图像的所述至少一个相应匹配特征点是通过如下方式获取的：在所述当前帧彩色图像中检测所述至少一个特征点，并且针对所检测到的所述至少一个特征点中的每个特征点，在所述相邻帧彩色图像中查找其相应匹配特征点，从而获取所述相邻帧彩色图像中的所述至少一个相应匹配特征点。

附记18.根据附记10所述的摄像机姿态估计装置，其中，所述当前帧深度图像的所述至少一个特征点和所述相邻帧深度图像的所述至少一个相应匹配特征点是通过如下方式获取的：

将所述当前帧彩色图像中的所述至少一个特征点映射到所述当前帧深度图像中，以获取所述当前帧深度图像中的所述至少一个特征点；以及

将所述相邻帧彩色图像中的所述至少一个特征点映射到所述相邻帧深度图像中，以获取所述相邻帧深度图像中的所述至少一个特征点。

Claims (10)

1.一种摄像机姿态估计方法，包括：

通过摄像机获得彩色图像和与所述彩色图像对应的深度图像；

获取当前帧彩色图像中的至少一个特征点和与所述至少一个特征点中的每个分别匹配的相邻帧彩色图像中的至少一个相应匹配特征点；

获取与所述当前帧彩色图像中的所述至少一个特征点中的每个的位置分别对应的当前帧深度图像中的至少一个特征点，并且基于所述相邻帧彩色图像中的所述至少一个相应匹配特征点，获取与所述当前帧深度图像中的所述至少一个特征点中的每个分别匹配的相邻帧深度图像中的至少一个相应匹配特征点；以及

在世界坐标系下计算所述当前帧深度图像中的每个特征点和所述相邻帧深度图像中的相应匹配特征点之间的距离，并且通过使得针对每对匹配的特征点所计算出的距离之和最小来估计当前摄像机姿态。

2.根据权利要求1所述的摄像机姿态估计方法，其中，

将第一帧深度图像的摄像机坐标系设置为世界坐标系；以及

摄像机姿态为用于将摄像机坐标系变换到世界坐标系的变换矩阵。

3.根据权利要求1所述的摄像机姿态估计方法，其中，基于所述当前帧深度图像中的每个特征点的世界坐标和所述相邻帧深度图像中的相应匹配特征点的世界坐标，计算所述当前帧深度图像中的每个特征点和所述相邻帧深度图像中的相应匹配特征点之间的距离。

4.根据权利要求3所述的摄像机姿态估计方法，

其中，在估计出所述当前摄像机姿态的情况下，基于所估计的当前摄像机姿态来计算所述当前帧深度图像中的所有特征点的世界坐标；以及

对于所述当前帧深度图像中的每个特征点：

如果该特征点也存在于所述当前帧深度图像之前的深度图像中，则将该特征点在所述当前帧深度图像中的世界坐标与该特征点在所述当前帧深度图像之前的深度图像中的世界坐标进行平均，作为该特征点在所述当前帧深度图像中的更新后的世界坐标。

5.根据权利要求1所述的摄像机姿态估计方法，还包括：

将深度图像的序列划分成组；以及

对于每个组，基于与该组中的每帧深度图像的所有特征点和该组中的所有其他帧深度图像的所有相应匹配特征点有关的组几何约束，优化该组中的每帧深度图像对应的摄像机姿态。

6.根据权利要求5所述的摄像机姿态估计方法，其中，所述组几何约束为：

使得在世界坐标系下的、该组中的所有对匹配特征点之间的距离之和最小。

7.根据权利要求1所述的摄像机姿态估计方法，其中，在获取所述相邻帧彩色图像的匹配特征点时，只有在所述相邻帧彩色图像中的匹配特征点存在于所述当前帧彩色图像之前的预定数量的帧中的情况下，才将该匹配特征点确定为最终的匹配特征点，以提高特征点匹配的精度。

8.根据权利要求1所述的摄像机姿态估计方法，其中，所述当前帧彩色图像的所述至少一个特征点和所述相邻帧彩色图像的所述至少一个相应匹配特征点是通过如下方式获取的：在所述当前帧彩色图像中检测所述至少一个特征点，并且针对所检测到的所述至少一个特征点中的每个特征点，在所述相邻帧彩色图像中查找其相应匹配特征点，从而获取所述相邻帧彩色图像中的所述至少一个相应匹配特征点。

9.根据权利要求1所述的摄像机姿态估计方法，其中，所述当前帧深度图像的所述至少一个特征点和所述相邻帧深度图像的所述至少一个相应匹配特征点是通过如下方式获取的：

将所述当前帧彩色图像中的所述至少一个特征点映射到所述当前帧深度图像中，以获取所述当前帧深度图像中的所述至少一个特征点；以及

将所述相邻帧彩色图像中的所述至少一个特征点映射到所述相邻帧深度图像中，以获取所述相邻帧深度图像中的所述至少一个特征点。

10.一种摄像机姿态估计装置，包括：

彩色图像和深度图像获取单元，被配置成通过摄像机获得彩色图像和与所述彩色图像对应的深度图像；

彩色图像特征点获取单元，被配置成获取当前帧彩色图像中的至少一个特征点和与所述至少一个特征点中的每个分别匹配的相邻帧彩色图像中的至少一个相应匹配特征点；

深度图像特征点获取单元，被配置成获取与所述当前帧彩色图像中的所述至少一个特征点中的每个的位置分别对应的当前帧深度图像中的至少一个特征点，并且基于所述相邻帧彩色图像中的所述至少一个相应匹配特征点，获取与所述当前帧深度图像中的所述至少一个特征点中的每个分别匹配的相邻帧深度图像中的至少一个相应匹配特征点；以及

摄像机姿态估计单元，被配置成在世界坐标系下计算所述当前帧深度图像中的每个特征点和所述相邻帧深度图像中的相应匹配特征点之间的距离，并且通过使得针对每对匹配的特征点所计算出的距离之和最小来估计当前摄像机姿态。

Images