CN108537848A - 一种面向室内场景重建的两级位姿优化估计方法 - Google Patents

Abstract

本发明公开一种面向室内场景重建的两级位姿优化估计方法，在特征点法的基础上，融合图像的光度信息，来进行相机位姿的求解，同时利用估计的相机位姿作为初值，通过由局部到全局的两级位姿优化策略，进一步提高相机位姿求解的精度。

Description

一种面向室内场景重建的两级位姿优化估计方法

技术领域

本发明属于计算机视觉领域，研究了一种面向室内场景重建的两级位姿优化估计方法。

背景技术

同时定位和地图构建(Simultaneous Localization And Mapping，SLAM)是目前机器人领域研究的重点和难点，是移动机器人真正实现自主导航的关键技术之一。随着工业信息技术的发展，SLAM技术得到了越来越广泛的应用，如在室内机器人、增强现实、无人机和无人驾驶等诸多方面都发挥着十分重要的作用。

SLAM的核心问题是估计相机位姿，通过估计相机位姿来实现相机的定位与场景的重建。

现有的相机位姿估计方法主要分为两大类，一类是采用特征点求解的特征点法，另一类是不采用特征点的直接法。使用特征点求解相机位姿主要有三种方法：

(1)2D-2D的对极几何方法。假设从两张图中得到了一对匹配好的特征点，那么这对特征点满足对极几何约束。

如图1所示，X为三维空间中一点，x和x′为X在两帧图像上的成像点，根据对极几何约束，有：

x′^TK^-TEK^-1x＝0 (1)

式中，K为相机内参矩阵，E为本质矩阵。在上式中，只有E是未知的，根据之前得到的多对特征匹配点，我们可以求出本质矩阵E，然后对E进行SVD分解，便可以求得相机位姿。

(2)2D-3D的方法。在三维重建过程中，我们可以使用一些算法求得特征点的深度信息，或者直接使用深度传感器获取到深度信息。利用已知深度信息，就可以将第一帧图像的二维特征点转化成真实世界中的三维坐标。将此三维坐标投影到第二帧图像上，通过最小化重投影误差就可以求得相机位姿。目标函数为：

式中，T为最后求得的相机位姿；π()表示投影函数；n为匹配的特征点个数；X_i表示第一帧图像中特征点对应的三维点坐标；x′_i表示在第二帧图像上匹配的特征点坐标。该方法最少需要三对匹配点便可以求解出相机位姿，同时需要额外的一对匹配点来进行验证。

(3)3D-3D的方法。如果把每对匹配特征点的二维坐标都转化成三维空间坐标就可以得到一组3D-3D的坐标对应关系。我们可以使用ICP(Iterative Closest Point)算法来进行求解，求解的目标函数为：

式中，X_i表示第一帧图像中特征点对应的三维点坐标；X′_i表示第二帧图像上对应特征点的三维坐标。

使用直接法求解相机位姿的主要思想是不提取特征点，直接通过两帧图像上光度的差异来计算相机位姿。假设在第一帧图像上有一个像素点p₁，根据当前的相机位姿在第二帧图像上寻找与其对应的像素p₂，如果相机的位姿不够好，那么p₁和p₂的外观会有明显的差别。为了减小这个差别，优化相机位姿，寻找与p₁更相似的p₂。此时，通过最小化光度误差来对位姿进行优化，目标函数为：

上式中，m表示第一帧图像中像素点总数；I₁()、I₂()分别表示取第一帧和第二帧图像的亮度值；y_j表示第一帧图像中的像素位置；Y_j表示第一帧图像中像素点对应的三维点坐标；π()表示投影函数；T为要求的相机位姿。

通过特征点法或直接法，可以得到估计的相机位姿。但是，由于传感器噪声等各方面的原因，估计的相机位姿依然存在累积误差的问题，因此，需要使用优化的方法对估计的位姿进行优化来消除累积误差的影响。目前常用的相机位姿估计方法为在关键帧上使用闭环检测来构建位姿图，然后基于位姿图来对全部的相机位姿进行优化。

基于特征点的对应关系来求解相机位姿，求解精度依赖于匹配特征点的数量以及特征点匹配的准确性。很多情况下，并不能提取出足够多的特征点来进行计算，而且只用特征点忽略了图像中其它的有用信息。直接法不需要使用特征点，因此在特征点稀缺的情况下依然可以稳定工作，但是其鲁棒性较差。在位姿优化方面，仅仅使用位姿图来对相机位姿进行优化，并不能完全的消除累积误差。因此，本发明提出了一种面向室内场景重建的两级位姿优化估计方法来获取精确的相机位姿。

在现有的相机位姿求解方法中，不论特征点法还是直接法，虽然已经取得了不错的效果，但是仍然存在很多不足。特征点法无法处理特征缺失的情况，并且忽略了图像中大部分有用信息。直接法避免了特征点法的缺点但是鲁棒性较差。在位姿优化方面，只使用位姿图来进行位姿优化，并不能完全消除累积误差。因此，本发明提出了一种面向室内场景重建的两级位姿优化估计方法，提高了相机位姿求解精度。

发明内容

本发明提供一种面向室内场景重建的两级位姿优化估计方法，在特征点法的基础上，融合图像的光度信息，来进行相机位姿的求解，同时利用估计的相机位姿作为初值，通过由局部到全局的两级位姿优化策略，进一步提高相机位姿求解的精度。

附图说明

图1对极几何约束；

图2本发明流程图；

图3重投影误差示意图；

图4光度误差示意图；

图5局部关键帧共视关系。

具体实施方式

如图2所示，本发明提供一种面向室内场景重建的两级位姿优化估计方法，包含以下步骤：相机位姿求解、相机位姿的局部优化和相机位姿全局优化。下面具体讲解每一步骤的实施方法。

步骤1.数据准备

本发明使用对齐的RGB-D图像序列和相机内参矩阵K作为输入。同时考虑到相机在成像时存在畸变，我们根据相机成像原理对RGB-D图像做了去畸变处理，本发明考虑的畸变主要为径向畸变和切向畸变。

特征提取阶段，本发明采用ORB特征来对彩色图像进行特征提取。同时采用KNN算法来对提取的特征进行匹配。为了得到可靠的匹配点，本发明采用最小阈值+RANSAC算法来对特征匹配点进行过滤，以此来保证获得可靠的特征匹配点。本发明将所有的特征匹配点记作集合F，用N表示F中特征匹配的数量。

为了获取图像的光度信息并且不显著的增加计算量，本发明首先提取图像中梯度变化明显的像素点，然后提取其光度信息作为后续计算的输入。假设图像为I，I上某个像素点在X轴和Y轴上的梯度分别定义为：

那么，当

时，则认为该像素点为梯度变化明显的点。本发明将所有梯度变化明显的像素点记作集合L，用M表示L中像素点的数量。

步骤2.相机位姿求解

相机位姿就是每个视点下相机坐标系到世界坐标系的变换关系。本发明选定第一帧图像的相机坐标系为世界坐标系。为了求解相机位姿，本发明使用了相邻帧的特征点对应关系及光度信息。

为了避免在优化求解的过程中陷入局部最优，需要提供一个较好的初始位姿。本发明通过相机运动模型来对当前帧的相机初始位姿进行估计。相机的运动模型定义为：

T＝speed*T_pre (7)

式中，speed表示相机运动的速度，T_pre表示上一帧的相机位姿，*表示两个矩阵相乘。通过公式(7)，可以估计出一个粗略的相机位姿，方便之后的计算。

对于当前帧和上一帧的特征点信息，本发明采用重投影误差来进行计算。两帧之间的重投影误差其原理如图3所示：

对于一对匹配的特征点p₁和p₂，p₁对应的空间点为P，将P投影到第二帧图像上之后得到的点的位置为p₃。若相机位姿准确，则p₂和p₃应该为同一个点。若相机位姿不准确，投影点p₃和真实位置p₂之间会存在一个误差e，通过优化的方法不断优化相机位姿，使误差e最小，就能得到最优的相机位姿。

根据以上描述，首先定义一对特征点之间的重投影误差：

其中p_i示某个特征点在第二帧图像对应的真实像素坐标，P_i表示该特征点在世界坐标系中的三维坐标，T表示要求解的相机位姿，K表示相机内参，Z_i表示将P_i投影之后Z轴对应的坐标。

接下来考虑两帧图像中所有匹配的特征点信息，可以得到总的误差函数为：

其中，N表示两帧中所有匹配特征点的数量。

对于光度信息，本发明考虑两帧之间的光度误差。光度误差的原理如图4所示：

假设在前一帧图像中某个像素点v₁，其对应的空间点为V。V使用相机位姿投影到第二帧图像上之后的像素位置为v₂。若相机位姿准确，那么v₁和v₂的光度应该是一样的，误差为0。若相机位姿不准确，那么两者之间的光度存在误差。通过优化相机位姿，使两者之间的误差最小，就可以得到最优的相机位姿。

根据以上描述，某个像素的光度误差可以定义为：

其中，v_j表示第一帧图像中的像素坐标，V_j表示点v_j的空间点坐标，T表示要求解的相机位姿，K表示相机内参，Z_i表示将V_j投影之后Z轴对应的坐标。I₁()和I₂()表示取前一帧图像和当前帧图像对应像素点的光度信息。

多个像素对的光度误差函数之和为：

其中，M表示像素对总数。折中考虑计算效率及精度，本发明选择梯度变化明显的像素点对计算光度误差。

融合匹配特征点的重投影误差和像素对的光度误差，可求解相机位姿估值如下：

其中，λ为调节系数，用来调节重投影误差和光度误差在位姿估计中的比重。

步骤3.相机位姿的局部优化

局部优化的思想是使用与当前关键帧有共视关系的关键帧来优化上述相机位姿估值，原理如图5所示：

在图5中，三角形表示关键帧序列，假设当前关键帧为kf_i,与当前关键帧有共同视野的关键帧为kf_i-1、kf_i-2和kf_i-3，本发明称之为共视关键帧。图中圆圈表示局部地图点，局部地图点就是当前关键帧和共视关键帧可以看到的空间三维点。

将局部地图中的每一个空间三维点投影到每一个共视关键帧当中，都可以得到一个重投影误差。考虑所有的局部地图点，就可以得到整体的误差函数：

式中，H表示共视关键帧的数量；Q表示局部地图点的数量；θ_ij表示地图点X_j投影到第i个共视关键帧后是否有与其匹配的特征点，若有其值为1，否则为0；即为地图点对应的真实的像素坐标；表示将地图点X_j投影到第i个关键帧上的Z轴坐标；T_i ^*表示每个共视关键帧的位姿。

通过相机位姿的局部优化，可以同时优化局部关键帧的相机位姿。优化之后可以得到更加精确的相机位姿T**。

步骤4.相机位姿的全局优化

基于关键帧共视关系的局部优化通过减少局部累积匹配误差优化相机位姿。在此基础上，基于闭环检测的全局优化可通过减少全局累积匹配误差进一步优化相机位姿，实现两级位姿优化估计。

首先以关键帧为对象进行闭环检测，并构建位姿图。位姿图中的顶点对应关键帧在世界坐标系下经局部优化所得的位姿矩阵，顶点连线表示相应顶点之间的相对变换矩阵。本发明方法使用词袋模型和特征匹配进行闭环检测，即满足特征匹配内点数量大于30的条件，则形成闭环。相应地，在位姿图中添加连线，更新位姿图。

记位姿图为G＝<set-ver,set-edge>,其中set-ver代表位姿图顶点集合，set-edge代表边集并记边W_i,j∈set-edge连接的两个顶点为T_i**和则可定义该边对应的代价函数如下：

由此可定义位姿图所对应的全局代价函数如下：

通过优化公式(16)，得到如下的相机位姿全局优化结果：

从以上过程可以看出，本发明在求解相机位姿时，不仅仅使用了特征点信息，还使用了光度信息，这使得我们的方法在特征点过少时依然可以有效的计算相机位姿。同时，本发明充分考虑了关键帧的局部共视关系，通过局部共视关系修正相机位姿的误差。本发明使用修正的相机位姿在关键帧序列上进一步进行全局的优化，从而构成了一个由局部到全局的两级位姿优化方法。通过本发明，可以求得高精度的相机位姿。

本发明对上述方法进行了实验验证，并取得了明显的效果。本发明中使用的RGB-D图像序列来自于TUM官方网站。每个场景的数据都包含RGB图像和Depth图像，以及每一帧图像对应的真实相机位姿。为了验证本发明的有效性，使用绝对轨迹误差(AbsoluteTrajectory)来评估本发明所求相机位姿与真实位姿的差异，同时，我们还和几个经典的SLAM方法进行了比较，结果表明，本发明具有更好的精度和鲁棒性。结果如表1所示：

表1本文方法的绝对轨迹误差

由表1可以看出，PTAM在fr1_xyz和fr2_xyz两个数据集上取得了最好的效果，但是其鲁棒性太差，在其它数据集上并不能得到最终的结果。上表中，ours+特征点法一列是仅使用重投影误差计算相机位姿得到的结果，通过对比可以发现，本发明提出的方法，精度上有了明显提升，同时还具有很好的鲁棒性。

Claims (3)

1.一种面向室内场景重建的两级位姿优化估计方法，其特征在于，包含以下步骤：

步骤1.数据准备

使用对齐的RGB-D图像序列和相机内参矩阵K作为输入，采用ORB特征来对彩色图像进行特征提取，同时采用KNN算法来对提取的特征进行匹配；

步骤2.相机位姿求解

选定第一帧图像的相机坐标系为世界坐标系，同时使用相邻帧的特征点对应关系及光度信息，

通过相机运动模型来对当前帧的相机初始位姿进行估计，相机的运动模型定义为：

T＝speed*T_pre (7)

其中，speed表示相机运动的速度，T_pre表示上一帧的相机位姿，*表示两个矩阵相乘，通过公式(7)，可以估计出一个粗略的相机位姿，

对于当前帧和上一帧的特征点信息，采用重投影误差来进行计算，对于一对匹配的特征点p₁和p₂，p₁对应的空间点为P，将P投影到第二帧图像上之后得到的点的位置为p₃，若相机位姿准确，则p₂和p₃应该为同一个点；若相机位姿不准确，投影点p₃和真实位置p₂之间会存在一个误差e，通过优化的方法不断优化相机位姿，使误差e最小，就能得到最优的相机位姿，

根据以上描述，首先定义一对特征点之间的重投影误差：

其中，p_i示某个特征点在第二帧图像对应的真实像素坐标，P_i表示该特征点在世界坐标系中的三维坐标，T表示要求解的相机位姿，K表示相机内参，Z_i表示将P_i投影之后Z轴对应的坐标，

根据两帧图像中所有匹配的特征点信息，可以得到总的误差函数为：

其中，N表示两帧中所有匹配特征点的数量。

对于光度信息，两帧之间的光度误差计算如下：

假设在前一帧图像中某个像素点v₁，其对应的空间点为V，V使用相机位姿投影到第二帧图像上之后的像素位置为v₂；若相机位姿准确，那么v₁和v₂的光度应该是一样的，误差为0；若相机位姿不准确，那么两者之间的光度存在误差；通过优化相机位姿，使两者之间的误差最小，就可以得到最优的相机位姿，

根据以上描述，某个像素的光度误差可以定义为：

其中，v_j表示第一帧图像中的像素坐标，V_j表示点v_j的空间点坐标，T表示要求解的相机位姿，K表示相机内参，Z_i表示将V_j投影之后Z轴对应的坐标，I₁()和I₂()表示取前一帧图像和当前帧图像对应像素点的光度信息，

多个像素对的光度误差函数之和为：

其中，M表示像素对总数。折中考虑计算效率及精度，本发明选择梯度变化明显的像素点对计算光度误差，

融合匹配特征点的重投影误差和像素对的光度误差，可求解相机位姿估值如下：

其中，λ为调节系数，用来调节重投影误差和光度误差在位姿估计中的比重，

步骤3.相机位姿的局部优化

使用与当前关键帧有共视关系的关键帧来优化上述相机位姿估值；将局部地图中的每一个空间三维点投影到每一个共视关键帧当中，都可以得到一个重投影误差；根据所有的局部地图点，就可以得到整体的误差函数：

其中，H表示共视关键帧的数量；Q表示局部地图点的数量；θ_ij表示地图点X_j投影到第i个共视关键帧后是否有与其匹配的特征点，若有其值为1，否则为0；即为地图点对应的真实的像素坐标；表示将地图点X_j投影到第i个关键帧上的Z轴坐标；T_i ^*表示每个共视关键帧的位姿。

通过相机位姿的局部优化，可以同时优化局部关键帧的相机位姿，优化之后可以得到更加精确的相机位姿T^**，

步骤4.相机位姿的全局优化

采用基于闭环检测的全局优化，实现两级位姿优化估计。

2.如权利要求1所述的面向室内场景重建的两级位姿优化估计方法，其特征在于，步骤4具体为：

首先以关键帧为对象进行闭环检测，并构建位姿图，位姿图中的顶点对应关键帧在世界坐标系下经局部优化所得的位姿矩阵，顶点连线表示相应顶点之间的相对变换矩阵；使用词袋模型和特征匹配进行闭环检测，即满足特征匹配内点数量大于30的条件，则形成闭环，相应地，在位姿图中添加连线，更新位姿图，

记位姿图为G＝<set-ver,set-edge>,其中，set-ver代表位姿图顶点集合，set-edge代表边集并记边W_i,j∈set-edge连接的两个顶点为T_i ^**和则可定义该边对应的代价函数如下：

由此可定义位姿图所对应的全局代价函数如下：

通过优化公式(16)，得到如下的相机位姿全局优化结果：

3.如权利要求1所述的面向室内场景重建的两级位姿优化估计方法，其特征在于，步骤1体为：

采用最小阈值+RANSAC算法来对特征匹配点进行过滤，以此来保证获得可靠的特征匹配点，将所有的特征匹配点记作集合F，用N表示F中特征匹配的数量，

首先提取图像中梯度变化明显的像素点，然后提取其光度信息作为后续计算的输入，

假设图像为I，I上某个像素点在X轴和Y轴上的梯度分别定义为：

那么，当

时，则认为该像素点为梯度变化明显的点，

将所有梯度变化明显的像素点记作集合L，用M表示L中像素点的数量。