特征融合的RGB-D相机运动估计方法与流程

本发明属于机器视觉技术领域，具体涉及一种特征融合的rgb-d相机运动估计方法。

背景技术：

近些年来，随着图像处理技术的快速发展和各种视觉传感器的出现，基于视觉的移动机器人得到越来越多的关注。相比激光雷达、毫米波雷达等，视觉传感器可以获取更加丰富的环境信息，同时还能降低成本。视觉里程计(visualodometry，vo)是仅通过视觉传感器来估计相机或与其相连的本体(例如：汽车、人或移动机器人等)的运动过程。它是视觉同时定位与地图创建(vslam)的一个子问题，是移动机器人实现自主导航的核心问题。常用的视觉传感器，包括单目相机，双目相机，全景相机和rgb-d相机。rgb-d相机，如微软的kinect、primersense、华硕xtionprolive、奥比中光等已经引起了极大的兴趣，不仅是因为它的重量轻、成本低，更为重要的是该类相机能同时提供颜色和深度信息。深度信息可以解决单目相机中的尺度问题。与双目相机相比，rgb-d相机的物理测量节省了大量的深度信息计算过程。

目前，主流的相机运动估计方法大致分为特征点法(如sift、surf、orb等)和直接法。它们都对光照变化较为敏感，且特征点法易受到特征不均匀分布的影响，算法的鲁棒性较差，不能满足应用需求。

技术实现要素：

本发明的目的是为了解决现有相机运动估计方法中易受光照影响和不均匀特征分布带来较大噪声的问题。提出一种基于点和线特征融合的鲁棒性rgb-d相机运动估计方法。

为了实现上述技术目的，本发明的技术方案是，一种特征融合的rgb-d相机运动估计方法，包括以下步骤：

步骤一，二维点和直线特征提取：在rgb图片上，分别用特征点检测算法和直线分割检测算法来提取二维特征点和二维特征直线；

步骤二，特征反投影至三维并做不确定性分析：结合深度信息，利用针孔相机模型，将提取的二维特征点和二维特征直线反投影到三维，在高斯噪声分布的假设下，分别对三维点和三维直线进行不确定性分析；

步骤三，特征匹配：对于点特征，计算特征点检测算法描述子并对连续两帧的点特征进行匹配；对于直线特征，计算其均值标准差描述子并进行匹配；然后用随机采样一致性算法去除误匹配；

步骤四，相机运动的估计：利用不确定性信息，对相机运动做极大似然估计；通过列文伯格算法对问题的目标函数进行求解，得到相机的位姿。

所述的一种特征融合的rgb-d相机运动估计方法，所述的步骤一包括以下步骤：

对于一幅rgb图，通过特征点检测算法获得二维特征点，同时，通过直线分割检测算法获得二维特征直线，得到特征集合{pi,lj|i＝1,2,…,j＝1,2,…}，其中二维点pi＝[ui,vi]^t，二维直线[ui,vi]^t表示点pi的像素坐标，aj和bj为直线lj的两个端点。

所述的一种特征融合的rgb-d相机运动估计方法，所述的步骤二包括以下步骤：

步骤1，三维点特征与不确定性分析：

将图像中的二维点p通过针孔相机模型，反投影到三维得到三维点p：

其中(u,v)表示二维点p对应的像素坐标，d为二维点p对应的深度值，[cu,cv]^t为相机的光圈中心，fc为焦距；

以二维点p的噪声是均值为0，协方差为的高斯分布，二维点p的深度值d的噪声与测量值成二次函数关系，即δd＝c1d²+c2d+c3，常数系数c1、c2和c3通过实验获得，三维点的不确定性为：

其中，噪声的协方差矩阵雅克比矩阵

其中，i2是2维单位矩阵，δ为噪声方差；

步骤2，三维直线特征和不确定性分析：

将二维直线采样足够多个点，舍弃深度值异常的点，根据公式(1)计算剩余点的三维坐标；然后采用随机采样一致性算法耐去除由于深度噪声影响在二维直线反投影到三维之后出现的局外点，并得到拟合的三维直线方程；用l＝[a^t,b^t]^t来表示对应的三维直线，其中a^t,b^t是三维直线上的两个三维点；

通过计算这些二维采样点所对应的三维点与所估计的三维直线l的马氏距离来度量三维直线的不确定性；

三维点p＝[x,y,z]^t到三维直线l＝[a^t,b^t]^t的马氏距离定义如下：

其中，q∈l是直线l上的一个任意点；有q＝a+λ(b-a),则q的优化估计q^*,

定义

δ(p-l)＝p-q^*(4)

则三维点p到三维直线l的马氏距离为

三维直线由一组经过随机采样一致性算法处理后的三维点{pi,i＝1,…nl}构成，其中,p1和分别代表直线的两个端点，令三维直线的误差函数为

于是三维直线l的极大似然估计l^*等价于最小化下式：

其中∑p和δ(p-l)分别由公式(2)和公式(4)计算得到；利用列文伯格算法求解公式(7)的非线性最小化问题，优化后的三维直线的不确定性为

其中，

所述的一种特征融合的rgb-d相机运动估计方法，所述的步骤三包括以下步骤：

对于点特征，计算其surf描述子，通过描述子之间的相似度来匹配相邻两帧图像的点特征；对于直线特征，计算均值标准化描述子，然后来进行匹配。

所述的一种特征融合的rgb-d相机运动估计方法，所述的步骤四包括以下步骤：

用t表示相邻两帧的运动转换，有t(x):＝rx+t，其中r为旋转矩阵，t为位移向量，取旋转矩阵和位移向量的非0项组成一个六维向量ξ；

以作为相邻两帧f和f’的一组匹配后的三维点集，则是相邻两帧的三维直线特征匹配对；若三维点匹配对对应的真实物理点在前一帧的局部坐标下为xi三维点，以三维直线匹配对对应的真实物理直线为yj，那么，相机里程计系统中待优化的变量为

定义系统的误差函数包括点特征的误差和直线特征的误差，即

其中，点特征的误差为

直线特征的误差为

单个直线的误差为η(li-yi)＝[δ(ai,yi)^t,δ(bi,yi)^t]，且

设定目标为找寻使相机里程计系统系统误差最小的变量δ，即

其中，σf＝diag(σp,σl)，本发明利用列文伯格算法对上式(10)进行求解，得到相机运动的初始位姿；利用相机运动的初始位姿信息构建位姿图，slam后端利用位姿图，通过图优化的方法来优化相机的运动轨迹。

所述的一种特征融合的rgb-d相机运动估计方法，所述的步骤2中，舍弃深度值异常的点是舍弃深度值为空值的点。

本发明的技术效果在于：

(1)直线特征对光照变化不敏感，本发明将点特征和线特征相结合，大大提高运动估计的鲁棒性。在室内环境下，直线特征尤其丰富，非常容易提取环境中的直线特征，降低了算法的复杂度。

(2)本发明对三维点和三维直线进行不确定性分析，并利用不确定信息对相机运动做极大似然估计，通过优化得到相机位姿，为后端优化提供了有效合理的初始值。

附图说明

图1为为特征融合的rgb-d相机运动估计系统框图。

图2为二维直线反投影到三维空间的示例图。

图3为相机的位姿图。

具体实施方式

下面结合附图对本发明的实施方式作进一步的说明。

本发明提出了一种基于点和线特征融合的rgb-d相机运动估计方法，整个系统框图如附图1所示，包括以下步骤：

s1.二维特征提取：系统利用输入的rgb图分别提取二维点特征和二维直线特征。

在本发明中，通过surf即特征点检测算法获得一组二维点特征，同时，通过lsd(linesegmentdetector)直线分割检测算法来获得二维直线特征。对于一幅rgb图，其特征集合{pi,lj|i＝1,2,…,j＝1,2,…}，其中二维点pi＝[ui,vi]^t，二维直线[ui,vi]^t表示点pi的像素坐标，aj和bj为直线lj的两个端点。

s2.三维特征获取及不确定性分析：将二维特征反投影至三维，并分别对三维点和三维直线做不确定分析，并优化三维特征的估计。

(1)三维点特征与不确定性分析

通过针孔相机模型，将二维点特征反投影到三维。假设一个图像中的二维点p特征对应的深度值为d，它对应的三维点p如下：

这里[cu,cv]^t为相机的光圈中心，fc为焦距。

通过公式(1),可以将二维点特征反投影至三维空间得到三维点特征(如附图2所示)。rgb-d相机存在较大测量误差，其获取的三维点云的噪声主要来源于rgb测量误差和深度值测量误差。

假设二维点p的噪声是均值为0，协方差为的高斯分布。其中，i2是2维单位矩阵，δ为噪声方差。三维点p的深度值d的噪声与测量值成二次函数关系，即δd＝c1d²+c2d+c3，常数系数c1、c2和c3通过实验获得。假设二维点p的测量误差和深度值d的误差相互独立，三维点的不确定性为：

其中，噪声的协方差矩阵雅克比矩阵

(2)三维直线特征和不确定性分析

三维空间中的直线投影到二维仍然保持直线的特性。因此，本发明首先在二维rgb图中检测直线，然后反投影到三维空间。反投影的思想如图2所示，将二维直线采样足够多个点，根据公式(1)计算这些点的三维坐标，并舍弃那些具有无效深度值的点。由于深度噪声的影响，二维直线反投影到三维之后会出现局外点(如附图2中的小圆圈)，我们采用随机采样一致性算法(ransac)来去除，并得到拟合的三维直线方程。我们用l＝[a^t,b^t]^t来表示对应的三维直线，其中a^t,b^t是三维直线上的两个三维点。

通过计算这些二维采样点所对应的三维点与所估计的三维直线l的马氏距离来度量直线的不确定性。

三维点p＝[x,y,z]^t到三维直线l＝[a^t,b^t]^t的马氏距离定义如下：

其中，q∈l是直线l上的一个任意点。假设q＝a+λ(b-a),于是公式(3)的最小化问题等价于单变量二次函数最小化问题。经过推导和计算可以得到q的优化估计q^*，即

定义

δ(p-l)＝p-q^*(4)

于是，三维点p到三维直线l的马氏距离可写为

三维直线由一组经过随机采样一致性算法处理后的三维点{pi,i＝1,…nl}构成，其中,p1和分别代表直线的两个端点。令三维直线的误差函数为

于是三维直线l的极大似然估计l^*等价于最小化下式：

其中∑p和δ(p-l)分别由公式(2)和公式(4)计算得到；利用列文伯格算法求解公式(7)的非线性最小化问题。优化后的三维直线的不确定性为

其中，

s3.相邻两帧的特征匹配：包括点特征匹配和直线特征匹配。

对于点特征，本发明计算其surf描述子，通过描述子之间的相似度来匹配相邻两帧图像的点特征。对于直线特征，计算msld描述子，然后来进行匹配。

s4.运动估计：本发明利用点特征和直线特征，对相机运动位姿进行估计。

相邻两帧的相机运动包括旋转和平移。用t表示相邻两帧的运动转换，有t(x):＝rx+t，其中r为旋转矩阵，t为位移向量，取旋转矩阵和位移向量的非0项组成一个六维向量ξ。

假设作为相邻两帧f和f’的一组匹配后的三维点集，则是相邻两帧的三维直线特征匹配对。若三维点匹配对对应的真实物理点在前一帧的局部坐标下为xi三维点，以三维直线匹配对对应的真实物理直线为yj，那么，相机里程计系统中待优化的变量为

定义系统的误差函数包括点特征的误差和直线特征的误差，即

其中，点特征的误差为

直线特征的误差为

单个直线的误差为η(li-yi)＝[δ(ai,yi)^t,δ(bi,yi)^t]，且

设定目标为找寻使相机里程计系统系统误差最小的变量δ，即

其中，σf＝diag(σp,σl)。本发明利用lm算法对上式(10)进行求解，得到相机运动的位姿。相机的位姿图如图3所示。相机初始位姿估计的信息可以构建位姿图，通过图优化方法来优化相机的轨迹。