视觉SLAM中的对极约束、三角测量、PnP、ICP问题

发布时间:2021-06-18 09:17:43

??这篇博客是在学*高翔《视觉SLAM十四讲》过程中对计算机视觉的多视图几何相关知识点做的总结,个人觉得这部分内容比较难,有理解不对的地方请指正!


?、ORB特征点

??对于特征点法的SLAM来说,ORB(Oriented FAST and Rotated BRIEF)特征应该是目前最合适的特征点了。ORB与SIFT、SURF之间的性能对比如下:


计算速度:ORB >> SURF >> SIFT(各差一个量级)旋转鲁棒性:SURF > ORB ~ SIFT模糊鲁棒性:SURF > ORB ~ SIFT尺度变换鲁棒性:SURF > SIFT > ORB(ORB算法在尺度方面效果较差)

??特征点由关键点和描述子两部分组成,对于ORB特征点来说,它使用的是FAST关键点+BRIEF描述子。


??FAST是一种角点,主要检测局部像素灰度变化明显的地方,以速度快著称。但FAST角点本身并不具有尺度和方向信息,针对这一问题,ORB算法通过构建图像金字塔,并在金字塔的每一层上检测角点来实现FAST的尺度不变性;用灰度质心法来实现FAST的旋转不变性。在ORB中,将这种可以描述角点尺度与旋转特性的FAST称为Oriented FAST。


??BRIEF是一种二进制描述子,为关键点添加描述子的作用是为了实现图像之间的特征匹配。BRIEF使用了随机选点的比较,速度非常快,而且由于使用了二进制表达,存储起来也十分方便。但原始的BRIEF描述子不具有旋转不变性,图像发生旋转时容易丢失,针对这一问题,ORB算法利用在FAST角点提取阶段计算出的关键点方向信息,使BRIEF具有了较好的旋转不变性。


??有了ORB特征点,我们需要用它来做图像间的特征匹配。所谓特征匹配,就是比较特征点的相似度,这个相似度通过它们的描述子来计算。对于BRIEF这种二进制的描述子,通常使用汉明距离(Hamming distance)来度量两个特征点之间的相似程度。汉明距离是指两个长度相等的字符串之间不同数据位的个数,例如:两个8位的二进制串00110100与10110100,它们之间只有第一个数据位不同,所以它们的汉明距离为1。在ORB的特征匹配中,汉明距离越小,特征点的相似度越高,在某种程度上,我们可以认为两个特征点在两帧图像中为同一个点,即完成了特征匹配。


??特征匹配在视觉SLAM中真的是至关重要的一步,设想一下,假如每一帧图像都能实现100%的正确匹配,那我们就不再需要各种令人头疼的滤波、优化、回环检测等等,这将是一个多么美好的画面啊啊啊。。。但,现实告诉你,绝大部分的图像之间是不可能做到100%正确匹配的,所以说,SLAM的主要工作在后端,就是如何利用不准确的数据估计得到准确的结果。


??当然,下面总结的在视觉SLAM中经常用到的,非常重要的,Multiple View Geometry in Computer Vision中的,比较难以理解又容易混淆的几个算法,都是在理想的特征匹配结果之上推导出来的。


一、对极几何

??对极几何(Epipolar geometry)又叫对极约束,是根据图像二维*面信息来估计单目相机帧间运动或双目相机相对位姿关系的一种算法。直观来讲,当相机在两个不同视角对同一物体进行拍摄时,物体在两幅图像中的成像肯定会有不同,那么,根据这两幅不同的图像,我们如何判断出相机的位姿发生了怎样的变化,这正是对极几何要解决的问题。


??需要明确的是,在对极几何中,我们的已知条件仅仅是每幅图像中特征点的像素坐标,当然,计算对极约束的前提是我们必须知道两幅图像中特征点之间准确的匹配关系。







图1 对极几何示意图

??如图1所示,以其中一对匹配点为例。




P



P


P为空间中的一点(坐标未知),其在左边图像中的投影为






p


l



=


[



u


l



,



v


l



,


1



]


T





color{#F00}p_l=[u_l,v_l,1]^ ext T


pl?=[ul?,vl?,1]T,在右边图像中的投影为






p


r



=


[



u


r



,



v


r



,


1



]


T





color{#F00}p_r=[u_r,v_r,1]^ ext T


pr?=[ur?,vr?,1]T,当我们以相机坐标系





O


L




O_L


OL?为参考坐标系时,有:







(1)






{








s


l




p


l



=


K


P











s


r




p


r



=


K


(


R


P


+


t


)






  

?
  


{








s


l




K



?


1





p


l



=


P











s


r




K



?


1





p


r



=


R


P


+


t












egin{cases} s_lp_l = KP \ s_rp_r = K(RP+t) end{cases}implies egin{cases} s_lK^{-1}p_l = P \ s_rK^{-1}p_r = RP+t end{cases} ag{1}


{sl?pl?=KPsr?pr?=K(RP+t)??{sl?K?1pl?=Psr?K?1pr?=RP+t?(1)


其中





s


l




s_l


sl?、





s


r




s_r


sr?分别为点




P



P


P在相机坐标系





O


L




O_L


OL?、





O


R




O_R


OR?中的




z



z


z坐标值(即深度),




K



K


K为




3


×


3



3 imes 3


3×3的相机内参矩阵,




R



R


R、




t



t


t即为





O


L




O_L


OL?与





O


R




O_R


OR?之间的相对位姿关系。取






x


l



=



K



?


1





p


l



,



x


r



=



K



?


1





p


r





color{#F0F}x_l=K^{-1}p_l,,x_r=K^{-1}p_r


xl?=K?1pl?,xr?=K?1pr?,则有:







(2)






{








s


l




x


l



=


P











s


r




x


r



=


R


P


+


t






  

?
  


s


r




x


r



=


R


(



s


l




x


l



)


+


t
  

?
  


s


r




x


r



=



s


l



R



x


l



+


t






egin{cases} s_lx_l = P \ s_rx_r = RP+t end{cases}implies s_rx_r = R(s_lx_l)+timplies s_rx_r = s_lRx_l+t ag{2}


{sl?xl?=Psr?xr?=RP+t??sr?xr?=R(sl?xl?)+t?sr?xr?=sl?Rxl?+t(2)


??上式两边同时左乘




t



t


t的反对称矩阵





t







t^wedge


t∧
,相当于两侧同时与




t



t


t做外积(




t



t


t与自身的外积





t






t


=


0



t^wedge t=f 0


t∧t=0):







(3)






s


r




t







x


r



=



s


l




t






R



x


l







s_rt^wedge x_r=s_lt^wedge Rx_l ag{3}


sr?t∧xr?=sl?t∧Rxl?(3)


??然后,两侧同时左乘





x


r


T




x_r^{ ext T}


xrT?:







(4)






s


r




x


r


T




t







x


r



=



s


l




x


r


T




t






R



x


l







s_rx_r^{ ext T}t^wedge x_r=s_lx_r^{ ext T}t^wedge Rx_l ag{4}


sr?xrT?t∧xr?=sl?xrT?t∧Rxl?(4)


??上式左侧中,




t



t


t与





x


r




x_r


xr?的外积





t







x


r




t^wedge x_r


t∧xr?是一个与




t



t


t和





x


r




x_r


xr?都垂直的向量,所以,再将





x


r


T




x_r^{ ext T}


xrT?与





t







x


r




t^wedge x_r


t∧xr?做内积,其结果必为




0



0


0,从而有:







(5)






s


l




x


r


T




t






R



x


l



=


0
  

?
  


s


l



(



K



?


1





p


r




)


T




t






R


(



K



?


1





p


l



)


=


0
  

?
  


p


r


T




K


-T




t






R



K



?


1





p


l



=


0






color{#F00}s_lx_r^{ ext T}t^wedge Rx_l=0implies s_l(K^{-1}p_r)^{ ext T}t^wedge R(K^{-1}p_l)=0implies p_r^{ ext T}K^{ ext {-T}}t^wedge RK^{-1}p_l=0 ag{5}


sl?xrT?t∧Rxl?=0?sl?(K?1pr?)Tt∧R(K?1pl?)=0?prT?K-Tt∧RK?1pl?=0(5)


??(5)式以非常简洁的形式描述了两幅图像中匹配点对





p


l




p_l


pl?和





p


r




p_r


pr?之间存在的数学关系,这种关系就叫对极约束。另外,我们称





E


=



t






R




color{#F00}E=t^wedge R


E=t∧R为本质矩阵(Essential Matrix),称





F


=



K


-T




t






R



K



?


1




=



K


-T



E



K



?


1






color{#F00}F=K^{ ext {-T}}t^wedge RK^{-1}=K^{ ext {-T}}EK^{-1}


F=K-Tt∧RK?1=K-TEK?1为基础矩阵(Fundamental Matrix),于是(5)式可以进一步简化为:







(6)






x


r


T



E



x


l



=



p


r


T



F



p


l



=


0






x_r^{ ext T}Ex_l=p_r^{ ext T}Fp_l=0 ag{6}


xrT?Exl?=prT?Fpl?=0(6)


??OK,现在我们需要重新明确一下对极几何的工作:对极几何是要利用已知的




n



n


n对如上述





p


l




p_l


pl?、





p


r




p_r


pr?这样的匹配点(二维像素坐标),来计算相机的运动




R



R


R、




t



t


t,即求解本质矩阵




E



E


E或基础矩阵




F



F


F。具体求解过程可参看《视觉SLAM十四讲》P143-148。


??对极几何存在的问题:根据对极约束求解得到的相机旋转运动




R



R


R是准确的,*移运动




t



t


t是不具备真实尺度的。


二、三角测量

??三角测量(Triangulation)又叫三角化,是根据前后两帧图像中匹配到的特征点像素坐标以及两帧之间的相机运动




R



R


R、




t



t


t,计算特征点三维空间坐标的一种算法。直观来讲,当有两个相对位置已知的相机同时拍摄到同一物体时,如何根据两幅图像中的信息估计出物体的实际位姿,即通过三角化获得二维图像上对应点的三维结构,这正是三角测量要解决的问题。







图2 三角测量示意图

??如图2所示为三角测量基本原理的示意图,该图与图1非常相似,但应该注意其中的区别:此时我们已经知道了





O


L




O_L


OL?与





O


R




O_R


OR?的关系,





p


l




p_l


pl?、





p


r




p_r


pr?也是已知的,要求的是点




P



P


P的三维空间坐标。由(1)式我们知道,要求点




P



P


P的三维坐标,需要先求深度




s



s


s;由(2)式我们有:







(7)






s


r




x


r



=



s


l



R



x


l



+


t






s_rx_r = s_lRx_l+t ag{7}


sr?xr?=sl?Rxl?+t(7)


对上式两侧左乘一个





x


r




x_r


xr?的反对称矩阵





x


r







x_r^wedge


xr∧?,得:







(8)






s


r




x


r







x


r



=



s


l




x


r






R



x


l



+



x


r






t
  

?
  


s


l




x


r






R



x


l



+



x


r






t


=


0






s_rx_r^wedge x_r = s_lx_r^wedge Rx_l+x_r^wedge timplies s_lx_r^wedge Rx_l+x_r^wedge t=f 0 ag{8}


sr?xr∧?xr?=sl?xr∧?Rxl?+xr∧?t?sl?xr∧?Rxl?+xr∧?t=0(8)


??(8)式是一个超定方程组(Overdetermined system),通常只能求得它的最小二乘解。当我们求出





s


l




s_l


sl?后,根据(7)式就很容易求出





s


r




s_r


sr?,最后由(1)式,我们有:







(9)





{








s


l




K



?


1





p


l



=



P



O


L















P



O


L





表示点



P



在相机坐标系




O


L




下的三维坐标?











s


r




K



?


1





p


r



=



P



O


R















P



O


R





表示点



P



在相机坐标系




O


R




下的三维坐标?











egin{cases} s_lK^{-1}p_l = P_{O_L}, & ext {$P_{O_L}$表示点$P$在相机坐标系$O_L$下的三维坐标 }\ s_rK^{-1}p_r = P_{O_R}, & ext {$P_{O_R}$表示点$P$在相机坐标系$O_R$下的三维坐标 } end{cases} ag{9}


{sl?K?1pl?=POL??,sr?K?1pr?=POR??,?POL??表示点P在相机坐标系OL?下的三维坐标?POR??表示点P在相机坐标系OR?下的三维坐标??(9)


??上式中





P



O


L





P_{O_L}


POL??与





P



O


R





P_{O_R}


POR??之间的关系为:







(10)






P



O


R




=


R



P



O


L




+


t






P_{O_R}=RP_{O_L}+t ag{10}


POR??=RPOL??+t(10)


即:以





O


L




O_L


OL?坐标系为参考,





O


R




O_R


OR?经过运动




R



R


R、




t



t


t变换到





O


L




O_L


OL?
,那么原来在





O


L




O_L


OL?坐标系下的点




P



P


P在





O


R




O_R


OR?坐标系下表示为





P



O


R




=


R



P



O


L




+


t



P_{O_R}=RP_{O_L}+t


POR??=RPOL??+t。如果我们还知道相机与世界坐标系的变换关系




T



T


T,就可以将





P



O


L





P_{O_L}


POL??或





P



O


R





P_{O_R}


POR??转换到世界坐标系下表示,得到点




P



P


P的世界坐标。


三、PnP问题

??PnP(Perspective-n-Point)是根据图像中特征点的二维像素坐标及其对应的三维空间坐标,来估计相机在参考坐标系中位姿的一类算法。直观来讲,当相机观察到空间中的某一物体时,我们已经知道了该物体在某一参考坐标系下的位置和姿态,那么如何通过图片中物体的成像判断出相机此时在参考坐标系下的位姿?这正是PnP要解决的问题,即利用已知三维结构与图像的对应关系求解相机与参考坐标系的相对关系(相机的外参)。


??PnP是一类问题,针对不同的情况有不同的解法,常见的算法有:P3P、DLS、EPnP、UPnP等。







图3 PnP示意图

??如图3所示,考虑空间中某个点




P



P


P,它在世界坐标系





O


w




O_w


Ow?中的齐次坐标为




P


=


[


x


,


y


,


z


,


1



]


T




P=[x,y,z,1]^{ ext T}


P=[x,y,z,1]T,其投影到图像中的像素坐标为




p


=


[


u


,


v


,


1



]


T




p=[u,v,1]^{ ext T}


p=[u,v,1]T。假设此时相机坐标系





O


c




O_c


Oc?与世界坐标系





O


w




O_w


Ow?之间的相对位姿关系由




R



R


R、




t



t


t来描述,则有:







(11)





s



[






u








v








1






]



=


K



[






R






t






]




[






x








y








z








1






]



=



[







f


x







0







u


0









0







f


y








v


0









0






0






1






]




[







r


11








r


12








r


13








t


1










r


21








r


22








r


23








t


2










r


31








r


32








r


33








t


3







]




[






x








y








z








1






]







color{#F00} segin{bmatrix} u \ v \ 1 \ end{bmatrix} = Kleft[egin{array}{c|c}R&tend{array} ight]egin{bmatrix} x\y\z\1 end{bmatrix} = egin{bmatrix} f_x&0&u_0 \0 &f_y&v_0\0&0&1end{bmatrix} left[ egin{array}{ccc|c} r_{11}&r_{12}&r_{13}&t_1 \ r_{21}&r_{22}&r_{23}&t_2 \ r_{31}&r_{32}&r_{33}&t_3 end{array} ight] egin{bmatrix}x\y\z\1end{bmatrix} ag{11}


s???uv1????=K[R?t?]?????xyz1??????=???fx?00?0fy?0?u0?v0?1???????r11?r21?r31??r12?r22?r32??r13?r23?r33??t1?t2?t3??????????xyz1??????(11)


其中的




R


=



[







r


1








r


2








r


3







]



=



[







r


11








r


12








r


13










r


21








r


22








r


23










r


31








r


32








r


33







]




R=egin{bmatrix}f r_1&f r_2&f r_3end{bmatrix}=egin{bmatrix}r_{11}&r_{12}&r_{13}\r_{21}&r_{22}&r_{23}\r_{31}&r_{32}&r_{33}end{bmatrix}


R=[r1??r2??r3??]=???r11?r21?r31??r12?r22?r32??r13?r23?r33?????、




t


=



[







t


1










t


2










t


3







]




t=egin{bmatrix}t_1\t_2\t_3end{bmatrix}


t=???t1?t2?t3?????即为我们要求的旋转矩阵与*移向量,其实它俩就是所谓的相机外参。通过相机的外参可以将世界坐标系下的点转换到相机坐标系下表示,通过相机的内参可以将相机坐标系下的点转换到像素坐标系下表示。


??为了简化表示,我们暂时不考虑相机内参(因为内参




K



K


K是已知且不变的),并令





T


34



=



[






R






t






]



=



[







r


11








r


12








r


13








t


1










r


21








r


22








r


23








t


2










r


31








r


32








r


33








t


3







]



=



[







t


11








t


12








t


13








t


14










t


21








t


22








t


23








t


24










t


31








t


32








t


33








t


34







]



=



[







τ


1










τ


2










τ


3







]




T_{34}=left[egin{array}{c|c}R&tend{array} ight]=left[egin{array}{ccc|c}r_{11}&r_{12}&r_{13}&t_1\r_{21}&r_{22}&r_{23}&t_2\r_{31}&r_{32}&r_{33}&t_3end{array} ight]=egin{bmatrix}t_{11}&t_{12}&t_{13}&t_{14}\t_{21}&t_{22}&t_{23}&t_{24}\t_{31}&t_{32}&t_{33}&t_{34}end{bmatrix} = egin{bmatrix}f{ au_1}\ f{ au_2}\ f{ au_3}end{bmatrix}


T34?=[R?t?]=???r11?r21?r31??r12?r22?r32??r13?r23?r33??t1?t2?t3?????=???t11?t21?t31??t12?t22?t32??t13?t23?t33??t14?t24?t34?????=???τ1?τ2?τ3?????,则上式可表示为:







s



[






u








v








1






]



=



[







t


11








t


12








t


13








t


14










t


21








t


22








t


23








t


24










t


31








t


32








t


33








t


34







]




[






x








y








z








1






]



=



[







τ


1










τ


2










τ


3







]



P



segin{bmatrix} u \ v \ 1 \ end{bmatrix} =egin{bmatrix}t_{11}&t_{12}&t_{13}&t_{14}\t_{21}&t_{22}&t_{23}&t_{24}\t_{31}&t_{32}&t_{33}&t_{34}end{bmatrix}egin{bmatrix}x\y\z\1end{bmatrix} = egin{bmatrix}f{ au_1}\ f{ au_2}\ f{ au_3}end{bmatrix}P


s???uv1????=???t11?t21?t31??t12?t22?t32??t13?t23?t33??t14?t24?t34??????????xyz1??????=???τ1?τ2?τ3?????P


??

相关文档

  • 小学一年级英语单词汇总
  • 将Vue项目部署至服务器(一)
  • 中学生晚自习如何计划
  • 机器学习笔记(一)逻辑回归与多项逻辑回归
  • 孕妇可以用护肤品吗
  • 可乐鸡翅怎么做比较好吃
  • 妈妈必须纠正食补的十大误区,孩子营养才更均衡
  • 最大的窃案打一成语
  • 苹果手机铃声
  • 牛背山旅游攻略
  • 观察蚂蚁的作文10篇
  • java中的StringUtil.isEmpty和StringUtil.isBlank
  • 八年级上册物理教学重难点
  • 信息网络中心主任竞争上岗演讲稿精选多篇
  • 做酒水批发起什么名字好
  • 上海聚焦世界科技发展前沿 多领域取得重大进展
  • 孕妇的宫缩症状
  • 主线程调用过环境检查以及防止游戏崩溃
  • 有关环保的演讲稿600字3篇
  • MongoDB4.2安装及启动
  • 黄河,母亲
  • 七一祝福语
  • 新来的数学老师
  • Java入门的前期准备
  • cmd简单游戏代码_python简单游戏应用??剪刀石头布!
  • 军校政治考核是如何组织的
  • 机械制造专业毕业生总结范文
  • 电力系统稳态分析第四版_电力系统稳态分析知识整理
  • 公司工作自我评价投稿15篇
  • 暴力王
  • 猜你喜欢

  • 预防突发环境污染事件对策浅析
  • 建筑节能工程进场材料、设备和构件验收记录表
  • 2018宁波最好的大学有哪些最新高校名单
  • 浅谈昭通市地面无线数字电视全覆盖工程
  • IATF16949质量控制内部审核检查表
  • 步步高vivo高通解锁工具_什么?vivo又出“智商检测机”?先别急着下定论
  • 鄂教版八下语文第一单元第2课《背影》教学设计word版
  • 我国酒店行业网络营销研究
  • 空间视域下《典型的美国佬》中的文化流变
  • 艾米丽.迪金森:如果记住就是忘却
  • 防抖和节流,你知道它们应用在哪些场景吗
  • 湖南省蓝山二中高中数学《3.1.2 两条直线的*行与垂直的判定》教案 新人教A版必修2
  • 教育最新K12河北省2019届中考数学系统复习 第四单元 图形的初步认识与三角形 第14讲 三角形的基础知识(8年
  • 调查问卷-社区教育调查报告(PPT38页)
  • 关于免交农民工工资保证金的申请报告 - 副本
  • 小学教师师德公开承诺书
  • 《落日怅望马戴》阅读答案
  • 我心目中的最美老师作文
  • 湄潭县复兴场供销合作社胡月秀农资经营部企业信用报告-天眼查
  • event对象的keyCode对应列表
  • 2018年企业和科研单位合作框架协议范本
  • 支教演讲致辞汇总
  • 最新计量经济学论文--中国粮食总产量多因素分析
  • 仪征市十二圩飞鸿酱品厂企业信息报告-天眼查
  • 餐饮点菜员岗位职责
  • 【数学】2014-2015年云南省曲靖市富源县竹园中学七年级上学期数学期中试卷和解析答案PDF
  • 2013年广州企业内部控制规范会计继续教育内容及答案
  • 【最新文档】雅思考试(IELTS)必备词汇短语英美用词的差异-推荐word版 (1页)
  • 布龙菲尔德与弗斯语言理论对比分析
  • 如何做一名的部门经理92487
  • 陈式太极拳老架一路74式拳谱共分为八段-(2)
  • 某村村务监督委员会的工作总结
  • 医学儿童EB病毒感染疾病的诊断指南PPT培训课件
  • 小学数学六年级毕业模拟试题及答案参考
  • 小学六年级叙事作文:一次乒乓球比赛
  • 电子工艺实习总结范文
  • 小学五年级作文《给身在异地的父母的一封信》1500字
  • 让自己漂亮,其实很简单!
  • 【教育资料】四年级下语文导学案-鱼游到了纸上人教新课标(无答案)学习专用
  • 一元洗车是什么意思
  • 优秀员工获奖感言1篇
  • KC 005 070000 041-类与对象汇总
  • 电脑版