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

8人阅读 评论(0) 收藏 举报
分类:

视觉里程计(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

查看评论

视觉里程计总结

立体视觉里程计左右图立体匹配常用方法: 1. 基于区域的匹配(构造小窗口,计算SAD,SSD,NCC等),能得到致密的视差图,但容易受到光照、图像旋转、尺度变化等因素的影响 2. 基于特征的匹配...
  • lgfun
  • lgfun
  • 2016-04-07 10:05:13
  • 1703

视觉里程计总介绍

转载地址:视觉里程计总介绍 在前两篇博客中,我们实现了一个简单的视觉里程计,在最后我们也分析了其存在的问题,那接下来的文章就是对上述简单视觉里程计一步步优化,提高算法的精度,效率以及鲁棒性。 在此之...
  • u011722133
  • u011722133
  • 2016-11-15 14:36:21
  • 2307

视觉里程计研究小结

1. 前言昨天在看甄嬛传的时候,被刚哥安利了他的研究课题:视觉里程计。然后丢给我两篇综述文章,让我研究下。然后,我居然放下了电视剧,看起了英文文献,oh,my god ~~文献原文地址: http:/...
  • zhyh1435589631
  • zhyh1435589631
  • 2016-12-10 23:40:15
  • 2968

视觉里程计(VisualOdometry)原理及实现

一、视觉里程计(VisualOdometry)介绍 目前,有不止一种方式可以确定移动机器人的轨迹,这里将重点强调“视觉里程计”这种方法。在这种方法中,单个相机或者双目相机被用到,其目的是为了重构出机器...
  • u013626386
  • u013626386
  • 2017-04-24 14:07:17
  • 3924

视觉里程计学习

初接触 初入门初接触大四要做毕业设计了,因为保送去了外校,决定毕业论文跟研究生导师做。研究生导师是做GPS导航的,想发展视觉导航方向,就想让我做视觉里程计,尝试把视觉定位研究一下。基本没有计算机视觉的...
  • mangao1746
  • mangao1746
  • 2017-02-24 17:12:54
  • 138

一个简单的视觉里程计实现(2)

前面讲述了特征提取,后期可以尝试orb_slam中采用orb,对特征加入方向信息,使得特征具备旋转不变性。 特征跟踪 特征提取是一个比较耗时的操作,即使使用了FAST特征检测,而且都是使用特征提取之后...
  • u011722133
  • u011722133
  • 2016-11-15 13:32:22
  • 2069

什么是视觉里程计(Visual Odometry)?

大牛讲堂|SLAM最终话:视觉里程计1.概念:什么是里程计?  在里程计问题中,我们希望测量一个运动物体的轨迹。这可以通过许多不同的手段来实现。例如,我们在汽车轮胎上安装计数码盘,就可以得到轮胎转动的...
  • weixin_37251044
  • weixin_37251044
  • 2018-01-09 14:36:38
  • 1343

视觉里程计的经典入门教程

  • 2016年12月13日 15:37
  • 6.19MB
  • 下载

里程计及基于双目视觉的视觉里程计(stereo visual odometry)

里程计在slam算法中扮演的作用不言而喻,通常里程计可以分为轮子编码器的里程计(wheel odometry)和视觉里程计(visual odometry)。而视觉里程计又可分为基于单目的视觉里程计(...
  • J10527
  • J10527
  • 2016-10-17 17:43:12
  • 5089

SLAM第二篇:视觉里程计

转载自:SLAM第二篇:视觉里程计 | ExBot易科机器人实验室 http://blog.exbot.net/archives/2546 上一讲中,我们介绍了组成SLAM系统的各个模块。以后,我...
  • fail_er
  • fail_er
  • 2017-06-08 11:30:04
  • 540
    个人资料
    持之以恒
    等级:
    访问量: 757
    积分: 231
    排名: 31万+
    文章存档