SLAM14讲学习笔记(四)视觉里程计(特征点法)和重难点总结

77
0
5天前

直接总结吧:这一节描述的视觉里程计就是检测特征点,特征点匹配,匹配上以后算位姿和深度。

特征点检测:

Fast特征点:以半径为3画圆,检测周围一圈一共16个点,设定阈值。有12个连续的点与该点亮度差超过阈值,确认为一个Fast特征点。(快速检测方式是先检查上下左右四个点,有三个点大于阈值才有可能是角点。因为要的是“连续”的点。有一个断掉了,就不能是连续的12个点了;然后通过非极大值抑制的方式筛选掉多余的点)

 

ORB特征点:改进fast特征点,具体是定义图像块的矩(mpq,p、q=0或1)

 

微信图片_20201111143749

 

再算质心,

 

微信图片_20201111143819

 

连接几何中心与质心得到方向向量作为特征点的方向。

 

微信图片_20201111143837

 

这个叫Oriented Fast 关键点。然后计算BRIEF描述子:比较关键点附近两个像素p和q的大小关系,用0和1表示。如果选128个p和q,就是128维二进制向量。

 

SIFT特征点:尺度不变特征变换(Scale-invariant feature transform):大致方法是首先搜索所有尺度下图像的位置,通过高斯微分函数来识别潜在的对于尺度和旋转不变的兴趣点,然后在候选位置通过拟合精细的模型来确定位置和尺度,再基于图像梯度,分配给关键点一个或多个方向,最后在每个关键点的周围邻域,在选定的尺度下测量图像局部梯度来描述关键点。(总的来说就是根据梯度描述关键点)

 

SURF特征点:加速稳健特征(Speeded Up Robust Features):加速版的SIFT。通过修改滤波器的尺寸和模糊度从而得到不同层级的影像,别的和SIFT差不多。

对于速度来说,ORB最快,SURF次之,SIFT最慢。

对于尺度变换的鲁棒性来说:SURF>SIFT>ORB(没有)

对于旋转模糊鲁棒性来说:SURF>ORB~SIFT

 

特征点匹配:

(1)暴力匹配,用汉明距离(描述子的不同位数)计算描述子距离,最近的作为匹配点。

(2)快速近似最近邻(FLANN)算法,这个书里没写细节。

 

初步筛选错误匹配:

经验方式:给定一个最小距离,如果汉明距离小于最小距离的两倍,就认为匹配没有太大的问题。

 


 

2D-2D

走到现在这里,特征点已经匹配出来了。也就是说,我们现在得到了一些像素点的配对(p1,p2)……想要根据这个恢复R和t。

 

接下来主要两个内容,对极几何求位姿,三角测量用求出来的位姿求深度。

 

对极几何

关于这块,高博的书里像素坐标用的是p1,p2,归一化平面坐标用的x1,x2。其实我觉得这两个符号换换更好。另外公式一堆,对于我来说,我觉得有点抓不住重点。我按适合自己的顺序总结一下:

 

微信图片_20201111143934

一个点,在两个归一化坐标平面表示为x1和x2,用R和t像上面这样建立起联系来。

 

两边都左乘t^,t^乘以一个向量,相当于t和这个向量做叉乘,产生的新的向量垂直于t和另一个向量(右手定律嘛)

另外,自己和自己叉乘结果是0

就得到了这个:

 

微信图片_20201111144001

 

这时候我们是想让它一边为0,好求R和t

注意等式左边,t^x2产生的向量,垂直于二者,那么t^x2再左乘一个x2T,就是两个垂直的向量求内积,结果是0

所以得到下式:

 

微信图片_20201111144031

 

上面这玩意就是对极几何,t^R表示成E,这个E就是本质矩阵(Essential Matrix)。

不过我们刚开始说了,特征点匹配以后得到的是两个像素坐标系下的点p1和p2,并不是x1和x2,两者之间差了个内参K的。

 

我们知道归一化平面上的点,左乘个内参就是像素平面上的点了,也就是说p=Kx,那么x就等于K-1 p

因此上面的式子变成了:

 

微信图片_20201111144051

 

把p留下来,中间的写成F,这个F就是所谓的基础矩阵(Fundametal Matrix)

 

所以总结为:两边是两个像素点,中间就是基础矩阵F(太基础了,不能再往深里化了);两边是两个归一化点,中间就是本质矩阵E。

 

其实E和F是一码事,因为内参K基本已知。二者求出一个就行了。

 

书中给出的是求本质矩阵E的过程。求法就是直接设一个E矩阵,九个点分别是e1……e9,两边设两个归一化的点的向量,展开乘,化出一个线性方程组的形式。大小是8*9。(除掉了一个常数,最后一个是1。实际上只需要最少5个点,因为t^R一共6个自由度,又去掉了一个常数。但是为了解这个方程,所以用8个点)

 

走到这里,E就求解出来了。但是E并不等同于位姿,因为E是t^R,想知道确切的位姿,需要拆开E,这个用的方法是SVD分解,就用上面说的E的奇异值性质,给了一个先验知识,说是E的奇异值满足[σ,σ,0]T的形式。分解出来是4个解,但是只有一个对于两个相机而言,都是正的深度。

 

关于这块,高博书里写的是相当的含糊,我这里放个截图:

 

微信图片_20201111144114

 

看了这些内容,我依旧不能明白怎么根据E求R和t。比如上面的RZ是什么,我就不理解。要求的不就是旋转矩阵吗?它和R1和R2有什么关系?特意去请教了一些师兄。

 

这块是比较难算的,因此高博书里没有展开介绍。

 

上面7.14式子里的两个RZ应该是不一样的,查阅别人的资料:https://www.cnblogs.com/CaptainLL/p/10900486.html

 

Matrix3d R_z1 = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); //定义旋转矩阵,沿 Z 轴旋转 90 度
Matrix3d R_z2 = AngleAxisd(-M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); //定义旋转矩阵沿 Z 轴旋转 -90 度

 

具体实现上面公示里的一样,Rz就是一个向z轴上方的向量,变成矩阵的形式,然后绕z轴旋转。

 

我观察了一下书上149页的代码,里面直接调用opencv的轮子recoverPose(),因此这块理解到这里应该就足够了。

查询了资料,补上这块的数学推导过程:https://blog.csdn.net/qq_30356613/article/details/80587649

用SVD之前,求出的E可能不满足奇异值是[σ,σ,0]T,需要刻意调整下。

 

面试容易考,E有5个自由度。

F矩阵书里没说,大概是因为E和F就差了一个内参,本质上是一码事。

但是,这块内容也得补充交代,因为面试会考!!借用浙大章国峰老师的PPT内容:

 

微信图片_20201111144205

 

F有7个自由度,原因是什么?因为F是3*3的矩阵,理论是9个自由度,但是由于秩为2,导致少了一个自由度,再加上尺度的自由度也没有,因此是9-2=7个自由度。

 

为什么秩为2?

因为:t^R相当于t^和R相乘,两矩阵相乘,其秩不大于各矩阵的秩。t^如下所示:

 

微信图片_20201111144243

 

第三列可以由前两列线性表示:第二列除以-Tz乘以Ty,第一列除以Tz乘以负Tx,之后相加。

因此秩是2。

F和E的求解过程,E最少五个点,F最少七个点,但是都用八点法来做。

 

单应矩阵H

单应矩阵(Homography)的意义在于避免“退化”现象造成基础矩阵自由度下降的问题。一般出现退化现象是因为特征点共面或者相机发生纯旋转。对于纯旋转的情况,E=t^R,纯旋转意味着t是0,那么E就是0,根据E恢复R就完全无从谈起。

对于特征点共面的情况,算H大概是更方便一些吧。

 

公式的推导颇有些奇妙,首先:

 

微信图片_20201111144311

 

这个公式就是一个面的公式,假如一个面的法向量是n=(A,B,C),还有一个点P(x,y,z)落在该面上,那么该面的公式就是:

Ax+By+Cz+d=0

 

然后根据这个式子,得到-nT*P/d=1,把这个式子乘到p=K(RP+t)的t后面,然后提出P,建立一个p1和p2的关系:

 

微信图片_20201111144344

 

把中间的记为H,那么p2=H*p1,H就是单应矩阵。(发现没有,假如没有t,中间算出的H就是KRK-1,直接就知道R了)

 

现在把这个公式和上面的F矩阵再做个对比:

 

F矩阵:

 

微信图片_20201111144406

 

H矩阵根据已经配对好的p1和p2,算中间的H,然后把向量和矩阵乘开,让h9为1,一组匹配点可以构造两项约束(这样八个未知数的项就都有了),用4对匹配点,算H。算出H以后,恢复Rt也是会得到四个解,最后也是排除得到R和t。自由度为8,四对匹配点,每一对匹配点贡献两个约束。(不过不能有三点共线的情况)

 

说到这里,对极几何就结束了,有四点注意:

1.单目尺度不确定性。t不晓得单位,因此需要初始化,让两张图像有一个平移,之后的平移都以这个平移为基本单位1。

2.虽然说如果没有t,可以用H算R,但是之后三角测量必须有t,所以最好左右平移,让它初始化。

3.上面求解的方法,都是直接求线性方程组,也就是“直接线性变换法”(DLT),如果匹配的点多了,直接解肯定是没解的,不唯一,这样可以用最小二乘法。另外还可以用Ransac算法解决误匹配的问题。我针对神经网络写过一个改进的Ransac算法,这里放上地址:https://github.com/Zkjoker/ICCV-PnPRansac_mutiprocessing

如果感兴趣可以帮我点个星!里面的细节留言我可以解决。

4.总结:E是SLAM14讲学习笔记(四)视觉里程计(特征点法)和重难点总结插图(13),x是归一化相机平面上的点,E有5个自由度,最少五个点,但是用8点法。

F是SLAM14讲学习笔记(四)视觉里程计(特征点法)和重难点总结插图(14),p是像素平面上的点,F有7个自由度,最少7个点,一般也用8点法。

H是SLAM14讲学习笔记(四)视觉里程计(特征点法)和重难点总结插图(15),p是像素平面上的点,H有8个自由度,最少四个匹配点,因为每对匹配点可以提供两个约束。

 

为什么要记这个东西呢,因为面试要考,不记不行。

三角测量

这块内容不多:

有同一个点在两个图里的归一化相机坐标x1和x2

 

现在都变到相机坐标系下,也就是都乘以各自的第三维s:

s1x1是x1位置的相机坐标系下的坐标,s2x2是x2位置的相机坐标系下的坐标

把s2x2变换到x1相机坐标系下,因为是同一个点,所以肯定相同:

 

微信图片_20201111144544

 

我们的目的是想求s1和s2,这时候要用技巧了,两边都左乘x1^,左边相当于自己和自己做叉乘,是0,右边就是:

 

微信图片_20201111144610

 

显然这里R和t在对极几何已经求完了,x1和x2又可以根据在图像上的位置求内参的逆矩阵求得,只有s2一个未知数。通过最小二乘可以算出来。

 

值得注意的是,平移太小,三角测量的精度不够;平移太大的话,特征点匹配又可能失效。这二者构成一对矛盾。

 

注意,在深蓝学院举办的视觉SLAM进阶-VIO视觉惯性里程计里面讲到的三角化,是这样的:

 

微信图片_20201111144647

 

这个和书中的三角测量还是有点区别的,有点像PNP那节的“直接线性变换法”(下面会说),y是世界坐标系下的坐标,u,v是像素坐标系下的坐标,P相当于是增广矩阵R|t,在多个位置观测到同一个点,联立上面的方程组,u1乘以第三行减去第一行,u2乘以第三行减去第二行……最后这样求解:

 

微信图片_20201111144717

 

取y=u4,即为这个最小二乘解,也是这个点在世界坐标系下的坐标。(数学证明我就不列出来了,之后我找完工作,或者找工作期间,会总结一个VIO的笔记,到时候在里面公布出来)

 

这块14讲里是没有的,希望同学们尽量看懂,这个补充看不懂就算了,以后再学也行。总之三角测量我们可以讲,仅仅通过一对匹配点来求位姿,三角化是多次观测到这个点,联立最小二乘,根据图像中的位置把它给算出来。反正二者是有区别的。

 

另外,不是特征点的像素点,就完全不能知道深度了吗?当然不是!具体理论见:SLAM14讲学习笔记(九)单目稠密重建、深度滤波器详解与补充(纠正第13讲 建图 中的错误),是通过极线搜索与块匹配找到特征点的对应关系,通过深度滤波器来实现深度值的更新与确定。

 


3D-2D

最开始先直接总结下:

这部分的内容的前提是一个点的3D坐标已知,另一个只知道在图像上的2D点坐标。

书里只介绍了直接线性变换法、P3P、非线性优化(Bundle Adjustment)三种。

第一种就是根据线性代数直接算,硬刚;第二种P3P是已知三个点的世界坐标和这三个点的像素坐标,把这三个点的相机坐标系下的坐标算出来,转换成ICP问题;第三种是非线性优化,最小化重投影误差来解决(这种是把相机位姿和空间点位置都看成优化变量)。

 

直接线性变化法:

 

微信图片_20201111144754

 

中间的t不是T,而是增广矩阵R|t。

这个里面,s未知,12个t未知,别的已知。都展开以后,前两行的约束里会有s,可以用第三行消掉它。

比如假设第一行是t1T,t1T*P=s*u1,s还等于t3T*P,把s代入,得到两个式子:

 

微信图片_20201111144826

 

把t1,t2,t3写成未知数的形式,用线性方程组求解。一对匹配点,才能都把t1,t2,t3包含进来,所以t有12维,得用6对匹配点。

求出增广矩阵以后,值得一提的是,需要对左边的3*3的矩阵块用一个最好的旋转矩阵来近似,可以用QR分解完成。

(可以注意到,这块和我上面补充的“三角化”的内容,是一个逆过程。三角化那块,意思是我把增广矩阵知道了,我就要根据二维点算深度。这块恰恰是反的,我知道了深度的3D点,我要求的是增广矩阵)

 

P3P:

这个看图比较头疼,简化了很多符号。总体上是用余弦定理做的:

 

微信图片_20201111144856

 

以上的图,OA,OB,OC是未知量。x=OA/OC,y=OB/OC,用余弦定理得到:

 

微信图片_20201111144925

 

把等式右侧的三项设为v,u*v,w*v,u=BC2/AB2,w=AC2/AB2是已知的,cos都已知,那么目前是x,y不知道,v不知道。把第一个式子代入后俩,消去v,式子变成:

 

微信图片_20201111144952

 

求出x,y以后,代入第上面三个式子中的第一个,可以求出v,v是AB2/OC2,就可以求出OC,别的也就依次都知道了。

 

关于这点,有几个值得注意:

1.cos值如何已知?

2D点图像位置已知,光心位置已知,可以计算其夹角,进而计算余弦。

 

2.求出的OA,OB,OC是什么?

可以当成是场景深度。场景深度和第三维的s是不一样的,s也是深度,但不是距离光心的深度,应该是所在平面距离光心所在平面的深度。但是可以互相求解。

 

3.之后怎么解位姿?

相机坐标系下3D坐标已知,可以据此求解3D-3D的ICP问题。

 

另外补充:还有EPNP算法书中没有介绍,感兴趣的同学可以阅读:SLAM学习笔记(十六)快速了解EPnP算法原理

Bundle Adjustment

这个内容是构建重投影误差,让重投影误差(观测值减去预测值)最小化。把相机位姿和空间点位置都看成优化变量,那么重投影误差函数相当于有两个自变量,一个是位姿,一个是对空间点P的位置求导。记P变换到相机坐标系下的位置是P’。

 

如果是对位姿求偏导,用扰动的方式,误差e先对P’求偏导,P’再对位姿的李代数求偏导,两个分开得到两个雅克比矩阵,然后乘在一起,得到一个2*6的雅克比矩阵。这样就求得了误差函数对位姿的导数矩阵。关于这里的雅克比矩阵,是比较重要的,因为g2o源码里显式的给出了雅克比矩阵的定义,如果不理解雅克比矩阵是怎么求出来的,代码就不能看懂。只当成黑盒子调用的话,理解不够深入。这个雅克比矩阵在后面的光度误差里面还会重复出现,因此我总结了一下:SLAM14讲学习笔记(五)视觉里程计(光流法和直接法)和对于雅克比矩阵的理解看到这里的人一定要看一下这节!

 

如果是对P求偏导,第一项也是e先对P’求偏导,第二项P‘对P求偏导结果就是个旋转矩阵R,因此这部分的导数矩阵就是第一项和R相乘。

 

综上,都是加一个小量扰动,然后通过求解对于小量的偏导,从而解决不能直接求偏导的问题。详情请看我的非线性优化这节的笔记:SLAM14讲学习笔记(三)非线性优化基础

 


3D-3D

这部分内容也是两种方式:代数方法和非线性优化方法。

代数方法是SVD方法,非线性优化还是类似于BA方式。

 

SVD方式:

 

微信图片_20201111145045

 

这部分内容只看上面的步骤,觉得有些抽象。其实定义的误差比较简单,两个3D点,坐标是P1和P2,P2’根据R*P1+t恢复得到,P2和P2‘应当是同一个点,定义二者之间的误差为e,构建误差平方和。

 

误差函数是经过特殊技巧处理的:

 

微信图片_20201111145131

 

自己加了又减去一些东西,最后化简得到的东西,等式右边三项,第一项只有R优化,优化它最小,就能得到R,代入得到第二项的t,第三项根据质心的定义为0(质心就是所有位置求和的均值),这样也就是上面的三个步骤了!

 

那么用代数方法优化SLAM14讲学习笔记(四)视觉里程计(特征点法)和重难点总结插图(27)该咋优化?

就是SVD方式。书里这直接给出了步骤,

把它拆开,得到三项:

 

微信图片_20201111145218

 

前两项与R无关,因为第二项R和自身转置乘积为1(R的固有性质),就是优化第三项。

 

第三项这么变:

 

微信图片_20201111145251

 

之后定义了W(∑后的东西),对W进行SVD分解,W满秩的时候,R=UVT,然后根据R恢复t。

书里代码所说,ICP目前还没有ICP算法,需要手动实现。

 

关于这里,我有两个疑问:

第一,等式左边到等式中间是怎么变的?迹有这种性质吗?

第二,为什么根据W的SVD分解,就可以得到R了?(在对极几何那节,本质矩阵E=t^R算出来以后,对E进行奇异值分解,那里我还能理解,虽然那个步骤我看不太懂,但是opencv里有轮子可以直接调用,稀里糊涂的也就知道R和t了。但是这节ICP的这里,根据一个目标函数一直推推推,最后挑出其中的一项作SVD分解,就直接求得了R和t吗?目标函数不应该是应该让它接近0吗?我没看到有这个步骤,好像是知道了每个点的去质心坐标,直接就求得W,然后算出R来了,没看到对目标函数优化的那个min步骤)

 

针对这两点问题,我特意又去看了高博的网课。

第一个问题,高博是这么说的:矩阵论里有,反正有这么个性质。(好吧)

第二个问题,他说“感兴趣的自己去看paper,反正步骤就是这么个步骤” (当然我一点也不感兴趣,我很不喜欢数学推导。他没有解释为什么没看到对目标函数优化的那个min步骤,我的理解是这样的:令目标函数为0,优化出来的结果,恰好就是这个R=UVT的形式)

 

注:最近学激光SLAM,又用到了ICP,因此对这里有了更深的理解。首先,不优化是因为这里用的是SVD方法,不是非线性优化的迭代步骤。至于为什么要这么求解(对W进行SVD分解),因为这里是想让RW的迹最大,从而-tr(RW)最小,然后总的和最小,最接近0。

 

微信图片_20201111145328

 

注意Track这里写错了,应该是Trace。意思是要构造一个正定对称矩阵,如果让R取这个VUT,那么可以使得RW的迹最大(取别的都没这个大)

 

非线性优化方法:

这块就是老生常谈了,对误差函数进行李代数扰动,迭代更新,最后找到合适的位姿。参见我在非线性优化那节的笔记。

 

值得注意的有两点:

1.有人已经证明,用ICP求解问题有唯一解或无穷多解情况。如果是唯一解的情况,那么极小值就是全局最优值。因此用ICP求解有一大好处:可以任意选定初始值。

2.可以混合使用PnP和ICP,深度已知就用ICP算法。

发表评论

后才能评论