视觉里程计(1)----libviso

视觉里程计(1)


数据集链接:Karlsruhe Dataset: Stereo Video Sequences + rough GPS Poses

github代码 :
https://github.com/srv/viso2
https://github.com/BrainSpawnInfosphere/libviso2


libviso2【转载】

 libviso一直以来被称为在视觉里程计(VO)中的老牌开源算法。它通过corner,chessboard两种kernel的响应以及非极大值抑制的方式提取特征,并用sobel算子与原图卷积的结果作为特征点的描述子。在位姿的计算方面,则通过RANSAC迭代的方式,每次迭代随机抽取3个点,根据这三个点,用高斯牛顿法计算出一个RT矩阵,表示两帧图像之间,相机的姿态变换。而位姿的计算也是libviso 中较为抽象的一部分,接下来,本文将在读者已经对立体视觉的基本原理,以及libviso的场景流匹配熟悉的前提下,对这个过程进行详细分析。

1 运动描述

libviso的实际位姿计算过程中,实质上是通过含有6个变量的向量

T={rx,ry,rz,tx,ty,tz}

来表示位姿变换的:
其中 rx,ry,rztx,ty,tz 分别表示两帧之间相机绕 x,y,z 轴之间的旋转和平移。计算出这6个变量之后,再转换成描述两帧之间位置变化的 R|t 矩阵。

2 位姿计算过程

在位姿计算的过程中,输入的是n组通过场景流匹配得到的二维坐标点,每组4个:
(u1c,v1c),(u1p,v1p),(u2c,v2c),(u1p,v1p)
分别代表左图、右图,当前时刻,上一时刻图像中的匹配点(这里用了与代码中相同的符号表示,1下标代表左图,2代表右图,c代表上一阵,p代表当前帧),
(X1c,Y1c,Z1c),(X1p,Y1p,Z1p),(X2c,Y2c,Z2c),(X1p,Y1p,Z1p)
是其对应的三维坐标,根据立体视觉的原理,三维坐标可以通过匹配点坐标结合相机内参数算出。输出是1中描述的6个变量。这6个变量是通过RANSAC迭代,在每次迭代中都从匹配点中随机抽取3个点,基于这3个点,通过高斯牛顿法的方式求出来的。计算出一次迭代中的参数之后,利用这个参数计算出局内点(inlier)的占比。最终取占比最高的参数,得到结果。下面将对每次迭代中进行的操作细节进行分析。

2.1 参数的更新

在每次迭代中,参数是通过梯度下降的方式求出来的。而高斯牛顿实质上也是一种通过迭代求解的方式。位姿解算的过程,可以看成是在RANSAC迭代中,再嵌套了一个迭代求解过程。高斯牛顿法计算的过程中,主要的计算工作包括两步:残差以及雅克比的计算,参数的迭代。
2.1.1 残差以及雅克比的计算

假设在当前的梯度下降迭代(第i次)中,参数的值为
Ti=rxi,ryi,rzi,txi,tyi,tzi
,当前的迭代,是根据RANSAC中抽取的3个点,更新这6个参数的值,使之更加接近正确解。

对于rxi,ryi,rzi首先,计算这三个量对应的旋转矩阵R,如下所示(为了简便,以下x指代rx,yz 以此类推)

Rx=[1000cos(x)sin(x)0sin(x)cos(x)]

Ry=[cos(y)0sin(y)010sin(y)0cos(y)]

Rz=[cos(z)sin(z)0sin(z)cos(z)0001]

R=RxRyRz=[cos(y)cos(z)cos(y)sin(z)sin(y)cos(x)sin(z)+cos(z)sin(x)sin(y)cos(x)cos(z)sin(x)sin(y)sin(z)cos(y)sin(x)sin(x)sin(z)cos(x)cos(z)sin(y)cos(z)sin(x)+cos(x)sin(y)sin(z)cos(x)cos(y)]

然后分别计算R中关于x,y,z的偏导数,如下所示:

这里写图片描述

这里写图片描述
假设X1p,Y1p,Z1p为左图匹配点对应的三维坐标,根据当前的参数R,tx,ty,tz,可以计算出在目前参数下,(X1c,Y1c,Z1c)的估计值:

[X1cY1cZ1c]=R[X1pY1pZ1p]+[txtytz]

根据对极几何原理,(X2c,Y2c,Z2c),可以通过以下方式求得

[X2cY2cZ2c]=[X1cbY1cZ1c]

其中b为立体相机基线长度。将(X1c,Y1c,Z1c),(X2c,Y2c,Z2c) 变换回对应的二维坐标的过程,称为重投影,具体计算方式如下:

u=fXZ

v=fYZ

给定一个重投影后的二维坐标点(u,v),其关于Ti的雅克比矩阵的计算方式如下:

Ju,v|T=[urxuryurzutxutyutzvrxvryvrzvtxvtyvtz]

其中,
urx=ZXrxXZrxZ2

v关于rx的偏导数以此类推Xrx表示的是Xrx方向上的偏导数,X,Y,Z为当前帧下的三维点坐标(即X1c,Y1c,Z1cX2c,Y2c,Z2c
通过上一帧的三维点以上述的公式计算可得,而其偏导数则通过下面的公式计算:

[XrxYrxZrx]=[XcrxYcrxZcrx]=(R[XpYpZp]+[txtytz])rx=Rrx[XpYpZp]

ry,rz的偏导数以此类推,由上述关于tx偏导数的表达式,可得:

[XtxYtxZtx]=[100]

基于上述计算方式,可以算出抽样得到的三组点关于(u1c,v1c)以及(u2c,v2c)的雅可比矩阵(对于每一组来说,都用一个46的矩阵来表示)。接下来,还需要计算残差,供最优化T使用
  对于一组点,残差实质上就是(u,v)的观测值(匹配点坐标)与其重投影后的坐标的差值,再乘以权重:
  
r=w[(u,v)(u^,v^)]

乘上权重w是为了减少标定参数不准确带来的误差,远离相机主点的特征点会赋予更低的权值,具体计算方式为:

w=cu|ucu|

2.1.2 高斯牛顿迭代
libviso的迭代过程中使用的高斯分布,实际上用了一个小技巧:将二次偏导数省略,只通过雅克比矩阵来进行迭代。通过计算三组点中的雅克比矩阵,最终,我们可以得到这样的一个矩阵:

J=[u1c1rxv1c1rxu2c1rxv2c1rx u1cNrxv1cNrxu2cNrxv2cNrxu1c1ryv1c1ryu2c1ryv2c1ry u1cNryv1cNryu2cNryv2cNryu1c1rzv1c1rzu2c1rzv2c1rz u1cNrzv1cNrzu2cNrzv2cNrzu1c1txv1c1txu2c1txv2c1tx u1cNtxv1cNtxu2cNtxv2cNtxu1c1tyv1c1tyu2c1tyv2c1ty u1cNtyv1cNtyu2cNtyv2cNtyu1c1tzv1c1tzu2c1tzv2c1tz u1cNtzv1cNtzu2cNtzv2cNtz]

N为抽样的点个数3,令
A=JJT
b=Jr
,其中r⃗ 为残差组成的向量:

r=[r1c1r2c1r1c2r1cNr2cN]

最后,高斯牛顿法通过下面的公式,进行T的迭代:

T(i+1)=T(i)+A1r

往复迭代,直到T收敛

|T(i+1)T(i)|<ε

2.2 最优位姿选取

从匹配点中选取3个,并通过高斯牛顿法,求出位姿T,即完成了一次RANSAC迭代。然而,每次迭代之后,我们都要判断计算出来的T的精确度。在libviso中,精确度是通过局内点(inlier)的个数来衡量的,若通过当前的位姿T

求得的局内点更多,就将这个位姿替换成当前最好的位姿。

  局内点的计算方式如下:先计算出(u^,v^),随后,满足下式的点即为局内点:
  
  i1c,2c(ui^ui)2+(vi^vi)2<t
  
 其中t为人为设定的参数。
 经过多次RANSAC迭代,最终能够快速鲁棒地找到两帧之间的位姿变换。这里的位姿变换是通过T
来表征的,而最终需要换算到R|t矩阵,所需要的公式在此不再赘述。

3 小结

本文详细地描述了在libviso中用到的位姿求解的过程。其主要思路是利用随机抽样,每次从匹配点中取出3组,进行高斯牛顿迭代,求出基于这3组点的位姿,再通过内点判断这个位姿的精确度,多次抽样——迭代后,得到一个准确的位姿。抽样能够能够加快高斯牛顿法的收敛速度,而RANSAC的思路则保证了这种抽样的鲁棒性,降低了动态点对算法的影响,是一种值得借鉴的思路。

libvisoproject主页位于
http://www.cvlibs.net/software/libviso/
,另外,我将其中的位姿求解模块单独抽取出来,供读者参考:
https://github.com/RichardChe/libviso_pose_estimation

阅读更多
版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/linwantian/article/details/79976058
个人分类: slam学习
想对作者说点什么? 我来说一句

没有更多推荐了,返回首页

关闭
关闭
关闭