CN113362377B - 一种基于单目相机的vo加权优化方法 - Google Patents

Abstract

一种基于单目相机的VO加权优化方法。1)通过KLT对图像进行特征点提取和匹配，并进行RANSAC剔除误匹配点，并进行关键帧筛选；2)通过八点法和三角化的方式，得到特征点的位置的初始值以及帧间位姿，逆深度；3)计算光度误差和逆深度误差，从而得到权重的初始值；4)构建最小二乘问题，准备优化变量特征点位置与相机的位姿；5)迭代运算，求得最优解，获得特征点位置与相机位姿。本发明方法通过利用匹配特征点的像素差和空间逆深度差服从t‑分布的特点，分别对每一帧的特征点进行加权，尽量降低像素差异大，在相机视野内较近和较远距离的特征点对全局优化的最优解的影响。

Description

一种基于单目相机的VO加权优化方法

技术领域

本发明涉及单目相机优化领域，特别是涉及一种基于单目相机的VO加权优化方法，是一种slam领域中针对对视觉匹配特征点进行加权优化的方法，可实现视觉slam高精度定位，去除了匹配较差精度的特征点对全局优化解带来的不利影响。

背景技术

近年来，随着机器人产业和无人驾驶产业的不断发展，同步定位与建图技术(SLAM)逐渐成为关键技术。SLAM方案分为前端里程计(VO)和后端建图部分。前端里程计主要负责根据环境信息提取载体的位姿信息。由于视觉信息丰富，漂移小，跟踪精度高的特点，使得视觉VO方案成为主流。其中视觉VO又可以根据特征提取方式的不同分为：特征点法和直接法。在特征点法中，通过提取每一帧的特征点，然后进行帧间匹配，获取帧间位姿和点的3D位置。随后根据帧间的匹配特征点的重投影误差，对特征点的位置，位姿进行全局优化。但是在视觉VO中，特征点的提取和匹配精度对后端优化结果有较大影响。对于变形程度较大，像素变化差异较大的特征点，进行跟踪后，若是和其余特征点一同优化，那么极有可能将全局最优解变为局部最优解，使得位姿优化得不到高精度的结果。因此，为了解决这一问题，在直接法中的一些方案中，通过对计算匹配后的像素点的光度误差，进行全局统计，得到像素光度差的方差，从而对每一个像素点的光度差进行加权。但是仅仅就像素差异进行加权，并没有考虑到像素点的空间深度的影响。在特征点法的一些方案中，有将光度误差和几何误差作为权重加入到重投影误差中，但是这仅是将其作为初始值代入，计算量较大，且对优化结果的改变并不明显。

发明内容

针以上问题，本发明提供一种基于单目相机的VO加权优化方法，在视觉VO中，由于不同特征点像素的差异和空间位置的差异，将会影响全局重投影误差的优化精度。该方法通过利用匹配特征点的像素差和空间逆深度差服从t-分布的特点，分别对每一帧的特征点进行加权，尽量降低像素差异大，在相机视野内较近和较远距离的特征点对全局优化的最优解的影响。

本发明提供一种基于单目相机的VO加权优化方法，其特征在于：包括以下步骤：

步骤1：通过KLT对图像进行特征点提取和匹配，并进行RANSAC剔除误匹配点，并进行关键帧筛选；

具体步骤如下，先是通过光流法KLT进行特征点匹配与跟踪，若是特征点数量低于阈值，则继续检测新的角点，随后通过RANSCAC法，使用基本矩阵模型，对KLT误匹配的特征点进行剔除，之后根据跟踪的特征点数量，当前帧和前一关键帧的平均视差和平均逆深度误差作为筛选关键帧的条件，若是以上其中一个低于阈值，则设为关键帧，否则继续对下一帧进行步骤1的操作；

步骤2：通过八点法和三角化的方式，得到特征点的位置的初始值以及帧间位姿，逆深度；

具体步骤如下，针对匹配好的特征点，解算初始位姿，在此之前，设相机的第一帧为世界坐标系，随后对滑动窗内的关键帧进行八点法解算本质矩阵，本质矩阵中包含着帧间旋转矩阵R和平移矩阵t的信息；通过SVD分解即可解算出R、t，在得到R、t之后，通过三角化方法计算出特征点的相机坐标系位置，X₁,X₂为特征点的相机归一化坐标，S₁,S₂为特征点的深度，λ为特征点的逆深度，因为逆深度更加服从高斯分布，因此在优化和加权项中使用逆深度；

E＝UΣ^-1V＝t^R

S₁X₁＝S₂RX₂+t

λ＝S^-1

步骤3：计算光度误差和逆深度误差，从而得到权重的初始值；

步骤3中根据位姿，可计算特征点的光度误差和逆深度误差，及其方差，并得到当前帧特征点光度权重和逆深度权重初始值，x为特征点的像素坐标，π()函数为将像素坐标转换为相机坐标系坐标P，τ()函数即取相机坐标系坐标P的逆深度；

x＝(u,v)

P₂＝RP₁+t

λ＝τ(P)；

步骤3中r_I为观测光度值，特征点为x₂和估计光度值特征点为x'₂的差值，r_λ为逆深度的观测值与估计值之差，由于光度误差和逆深度误差服从t分布，因此，得到光度权重和逆深度权重W_I,W_λ；其中v为自由度，a为常数系数，μ,σ分别为该帧图像所有特征点的光度误差或逆深度误差的期望和标准差，将步骤2解算出的R,t和逆深度代入，即可得到权重的初始值；

r_I＝I(x₂)-I(x'₂)

r_I＝I(x₂)-I(π^-1(Rπ(x₁)+t))

r_λ＝τ(π(x₂))-τ(Rπ(x₁)+t)

步骤4：构建最小二乘问题，准备优化变量特征点位置与相机的位姿；

具体步骤如下，将权重加入到重投影误差项中，W_i为第i个特征点的光度权重与逆深度权重的积，r_i,k为第K关键帧的第i个特征点的重投影误差；

r_i,k＝T_k,k-1P_i ^k-1-P_i ^k

因此便可构建第K帧的最小二乘问题，

为优化变量；

步骤5：迭代运算，求得最优解，获得特征点位置与相机位姿。

作为本发明进一步改进，步骤5中迭代运算求解；

1)给出步骤2计算出的优化变量初始值和权重的初始值；

2)对于第m次迭代，求出当前的雅克比矩阵

3)求解增量方程，得出Δξ；

4)若Δξ达到阈值，则停止优化；否则更新变量和权重值并返回第2步。

本发明与现有技术相比的优点在于：

本发明通过评估匹配特征点的光度和逆深度权重信息，对重投影优化项进行加权，可有效的避免匹配度较差的特征点对全局最优解的影响，有效的提高了VO的优化精度和对光照度变化的抗干扰性。通过逆深度误差作为关键帧的选取条件之一，可避免纯Z轴运动而检测不到出的问题。

附图说明

图1为本发明整体流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明提供一种基于单目相机的VO加权优化方法，通过计算匹配的特征点，计算光度误差和逆深度误差，得到对应特征点的权重信息，由此加入到重投影误差项中。

作为本发明具体实施例，整体流程图如图1所示，本发明的技术解决方案为：通过计算匹配的特征点，计算光度误差和逆深度误差，得到对应特征点的权重信息，由此加入到重投影误差项中。

步骤1：首先每一帧的图像进行KL光流法特征点提取，并进行KLT稀疏光流法进行特征点跟踪。同时若图像特征点的数量过少，则进行角点特征检测，以保证特征点的数量足够。随后利用基本矩阵模型的RANSAC算法对匹配的特征点进行误匹配剔除。同时根据当前帧与上一关键帧跟踪的特征点，计算平均视差，若视差超过阈值，则定当前帧为关键帧；或者跟踪的特征点数量低于阈值，也设为关键帧；若当前帧与上一关键帧的平均逆深度误差超过阈值，亦定位关键帧。

步骤2：在初始位姿解算中，先将相机的第一帧定为世界坐标系。对选取的关键帧进行滑动窗操作，通过八点法计算本质矩阵E，并通过SVD分解，解算出帧间位姿旋转矩阵R和平移矩阵t。在得到R、t之后，通过三角化方法计算出特征点的相机坐标系位置。X₁,X₂为特征点的相机归一化坐标，S₁,S₂为特征点的深度，λ为特征点的逆深度，因为逆深度更加服从高斯分布，因此在优化和加权项中使用逆深度。

本质矩阵E：E＝UΣ^-1V＝t^R

三角化：S₁X₁＝S₂RX₂+t

λ＝S^-1

步骤3：根据位姿，可计算特征点的光度误差和逆深度误差，及其方差，并得到当前帧特征点光度权重和逆深度权重初始值。x为特征点的像素坐标，π()函数为将像素坐标转换为相机坐标系坐标P。τ()函数即取相机坐标系坐标P的逆深度。

x＝(u,v)

P₂＝RP₁+t

λ＝τ(P)

r_I为观测光度值(特征点x₂)和估计光度值(特征点x'₂)的差值。r_λ为逆深度的观测值与估计值之差。由于光度误差和逆深度误差服从t分布，因此，可得到光度权重和逆深度权重W_I,W_λ；其中ν为自由度，a为常数系数，μ,σ分别为该帧图像所有特征点的光度误差或逆深度误差的期望和标准差。将步骤2解算出的R,t和逆深度代入，即可得到权重的初始值。

r_I＝I(x₂)-I(x'₂)

r_I＝I(x₂)-I(π^-1(Rπ(x₁)+t))

r_λ＝τ(π(x₂))-τ(Rπ(x₁)+t)

步骤4：将权重加入到重投影误差项中。W_i为第i个特征点的光度权重与逆深度权重的积。r_i,k为第K关键帧的第i个特征点的重投影误差。

r_i,k＝T_k,k-1P_i ^k-1-P_i ^k

因此便可构建第K帧的最小二乘问题，

为优化变量。

步骤5：迭代运算求解。1)给出步骤2计算出的优化变量初始值和权重的初始值；2)对于第m次迭代，求出当前的雅克比矩阵

3)求解增量方程，得出Δξ；4)若Δξ达到阈值，则停止优化；否则更新变量和权重值并返回第2)步。

以上所述，仅是本发明的较佳实施例之一，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (2)

1.一种基于单目相机的VO加权优化方法，其特征在于：包括以下步骤：

步骤1：通过KLT对图像进行特征点提取和匹配，并进行RANSAC剔除误匹配点，并进行关键帧筛选；

具体步骤如下，先是通过光流法KLT进行特征点匹配与跟踪，若是特征点数量低于阈值，则继续检测新的角点，随后通过RANSCAC法，使用基本矩阵模型，对KLT误匹配的特征点进行剔除，之后根据跟踪的特征点数量，当前帧和前一关键帧的平均视差和平均逆深度误差作为筛选关键帧的条件，若是以上其中一个低于阈值，则设为关键帧，否则继续对下一帧进行步骤1的操作；

步骤2：通过八点法和三角化的方式，得到特征点的位置的初始值以及帧间位姿，逆深度；

具体步骤如下，针对匹配好的特征点，解算初始位姿，在此之前，设相机的第一帧为世界坐标系，随后对滑动窗内的关键帧进行八点法解算本质矩阵，本质矩阵中包含着帧间旋转矩阵R和平移矩阵t的信息；通过SVD分解即可解算出R、t，在得到R、t之后，通过三角化方法计算出特征点的相机坐标系位置，X₁，X₂为特征点的相机归一化坐标，S₁，S₂为特征点的深度，λ为特征点的逆深度，因为逆深度更加服从高斯分布，因此在优化和加权项中使用逆深度；

E＝U∑^-1V＝t^∧R

S₁X₁＝S₂RX₂+t

λ＝S^-1

步骤3：计算光度误差和逆深度误差，从而得到权重的初始值；

步骤3中根据位姿，可计算特征点的光度误差和逆深度误差，及其方差，并得到当前帧特征点光度权重和逆深度权重初始值，x为特征点的像素坐标，π()函数为将像素坐标转换为相机坐标系坐标P，τ()函数即取相机坐标系坐标P的逆深度；

x＝(u，v)

P₂＝RP₁+t

λ＝τ(P)；

步骤3中r_I为观测光度值，特征点为x₂和估计光度值特征点为x′₂的差值，r_λ为逆深度的观测值与估计值之差，由于光度误差和逆深度误差服从t分布，因此，得到光度权重和逆深度权重W_I，W_λ；其中v为自由度，a为常数系数，μ，σ分别为该帧图像所有特征点的光度误差或逆深度误差的期望和标准差，将步骤2解算出的R，t和逆深度代入，即可得到权重的初始值；

r_I＝I(x₂)-I(x′₂)

r_I＝I(x₂)-I(π^-1(Rπ(x₁)+t))

r_λ＝τ(π(x₂))-τ(Rπ(x₁)+t)

步骤4：构建最小二乘问题，准备优化变量特征点位置与相机的位姿；

具体步骤如下，将权重加入到重投影误差项中，W_i为第i个特征点的光度权重与逆深度权重的积，r_i，k为第K关键帧的第i个特征点的重投影误差；

r_i，k＝T_k，k-1P_i ^k-1-P_i ^k

因此便可构建第K帧的最小二乘问题，

为优化变量；

步骤5：迭代运算，求得最优解，获得特征点位置与相机位姿。

2.根据权利要求1所述的一种基于单目相机的VO加权优化方法，其特征在于：步骤5中迭代运算求解；

1)给出步骤2计算出的优化变量初始值和权重的初始值；

2)对于第m次迭代，求出当前的雅克比矩阵

3)求解增量方程，得出Δξ；

4)若Δξ达到阈值，则停止优化；否则更新变量和权重值并返回第2步。

Images