SLAM|视觉SLAM十四讲-第七讲笔记

主要内容 本章开始进入视觉里程计(VO)部分,VO按是否需要提取特征,分为特征点法的前端和不提特征的前端。这一章讲的是基于特征点法的前端,分为以下内容。

  1. 特征点法:找到两张2D图像上的匹配点。
  2. 对极几何:根据2D-2D特征点对求解R,t。
  3. 三角测量:根据2D-2D特征点求深度。
  4. PnP:根据3D点云和匹配的2D图像求R,t。
  5. ICP:求两个点云之间的R,t。
关系是:
  1. 特征点法找到2D图像的匹配点对,用于对极几何和pnp
  2. 对极几何求出2D-2D的位姿。
  3. 根据对极几何求出的位姿,三角测量求出2D-2D的深度。
  4. 根据三角测量求出的深度,可以初始化单目SLAM,得到三维点信息;或用RGBD相机获得三维信息。知道3D信息,对于下一张2D图像,可根据特征点法找到的匹配点对,使用PNP,求出位姿信息和深度信息。
  5. 根据pnp求出的深度信息;或直接用RGBD得到的两个点云,可以用ICP,求出两个点云之间的位姿变换。
SLAM|视觉SLAM十四讲-第七讲笔记
文章图片

图片引自leeyau的博客
一、特征点法 这部分内容,讲述了提取2D图像的特征点,并对不同图像之间的特征点进行匹配,得到匹配的特征点对。这些匹配点是后面用来估计相机位姿的基础。
特征点:在SLAM中被成为路标。分为关键点和描述子。
对于SLAM来说,SIFT太慢,FAST没有方向信息,使用改进FAST的ORB特征。
ORB特征
关键点:Oriented FAST 是改进的,具有方向性的FAST描述子。
FAST用于检测局部像素灰度明显变化的位置,思想是看邻域内有没有连续N个和它差T灰度的点。加上NMS和其他遍历的加速trick。
ORB的改进:
  • 根据FAST关键点的Harris响应值,选取前N大的,以固定特征点数目。
  • 构建图像金字塔,解决尺度问题。
  • 使用灰度质心法,解决旋转问题。(加一个几何中心到质心的向量)
描述子:BRIEF 是一种二进制描述子,描述了关键点附近两个像素的大小关系。
特征匹配
局部特征会导致误匹配,难以解决。
BRIEF描述子使用汉明距离进行相似性度量,快速近似最近邻(FLANN)适合数目多的特征点匹配。
二、对极几何 SLAM|视觉SLAM十四讲-第七讲笔记
文章图片

  • 已知:匹配点对 p 1 p_1 p1?, p 2 p_2 p2?的像素坐标。
  • 给定:两张二维图像,二维图像上特征点的匹配关系。
  • 未知:P的三维空间坐标, I 1 I_1 I1?到 I 2 I_2 I2?的变换矩阵( T 1 , 2 T_{1,2} T1,2?,即 R , t R,t R,t)。
【SLAM|视觉SLAM十四讲-第七讲笔记】一般是用于__单目SLAM的初始化__,用对极几何可求出位姿,在用三角测量估计三维空间点的位置后,就能用其他更准确的方法继续求解了。
1. 对极约束
设P在图1的相机坐标系下,坐标为:
P = [ X , Y , Z ] T P=[X,Y,Z]^T P=[X,Y,Z]T
p 1 p_1 p1?, p 2 p_2 p2?的像素坐标(单位像素):
s 1 p 1 = K P , s 2 p 2 = K ( R P + t ) s_1p_1=KP, s_2p_2=K(RP+t) s1?p1?=KP,s2?p2?=K(RP+t)
其归一化平面坐标(单位米):
x 1 = K ? 1 p 1 , x 2 = K ? 1 p 2 x_1=K^{-1}p_1,x_2=K^{-1}p_2 x1?=K?1p1?,x2?=K?1p2?
得到:
x 2 = R x 1 + t x_2=Rx_1+t x2?=Rx1?+t
这里的 x x x是齐次坐标,等式表达了一个齐次关系。
两边同时左乘 t ‘ {t‘} t‘,这个东西相当于与它做外积,得到的结果和它、 x 2 x_2 x2?都垂直。因此再左乘一个 x 2 T x_2^T x2T?,就得到了0。
最终得到:
x 2 T t ’ R x 1 = 0 x_2^Tt’Rx_1=0 x2T?t’Rx1?=0
中间部分写成 E E E矩阵,称为本质矩阵:
x 2 T E x 1 = 0 x_2^TEx_1=0 x2T?Ex1?=0
带入 p 1 p_1 p1?, p 2 p_2 p2?:
p 2 K ? T t ’ R K ? 1 p 1 = 0 , E = t ‘ R p_2K^{-T}t ’RK^{-1}p_1=0,E=t^{‘}R p2?K?Tt’RK?1p1?=0,E=t‘R
中间写成 F F F矩阵,称为基础矩阵:
p 2 F p 1 = 0 , F = K ? T t R K ? 1 p_2Fp_1=0,F=K^{-T}tRK^{-1} p2?Fp1?=0,F=K?TtRK?1
t’是t的反对称矩阵。
这两个式子称为对极约束。根据对极约束,估计 R , t R,t R,t的方法为:
  1. 根据匹配点的位置,求出 E E E或者 F F F。
  2. 根据 E E E或者 F F F,求出 R , t R,t R,t。
2. 求解本质矩阵E
本质矩阵长什么样子:
  • 大小为3x3
  • 由平移和旋转定义,共有3+3=6个自由度
  • 因为对极约束,E具有尺度等价性,自由度-1,共五个。
八点法 根据对极约束,最少五对点就能求出来E,为了方便,一般使用八对点。
写出由八对点构成的线性方程组,可以解出来E。
估计相机运动
对E做SVD,就能求出来 R , t R,t R,t。可以得到四组解(深度正/负,相机位置的左/右):
SLAM|视觉SLAM十四讲-第七讲笔记
文章图片

因为深度要是正的才合法,可以把解带入一个点看看深度是否都为正(用三角测量?),从中选出最终解。
3. 单应矩阵
描述了两个平面之间的映射关系。
单应矩阵:H。自由度为8,可通过四对匹配特征点算出。
单应性:当特征点共面,或发生纯旋转时,自由度下降。
4. 单目SLAM的一些问题
尺度不确定性 t的尺度不确定性。对t乘任意倍数,对极约束依然成立。
单目SLAM的初始化:对t进行归一化,固定尺度,或吃实话时,所有特征点的平均深度为1。
纯旋转问题 若是纯旋转,t为0,则E为0,无法求解R。
单目初始化不能只有旋转,必须要有平移。
多于八对点 计算最小二乘解。
或使用RANSAC,来避免误匹配的影响。
5. 三角测量
单目SLAM(2d-2d)中,使用对极约束估计 R , t R,t R,t,使用三角测量估计深度。
三、3D-2D PnP
  • 给定:一张已知特征点3D位置的图像,一张二维图像。
  • 已知:特征点在世界坐标系(指的是相机最初位置的那个坐标系)下的三维坐标,特征点映射到图像上的二维坐标,三维- - 坐标系中的特征点与二维图像上点的匹配关系。
  • 未知:相机坐标系在世界坐标系下的位姿,特征点在相机坐标系下的深度。
  • 最初使用对极几何,初始化出特征点的深度。对于后续的二维图像(当前帧),当前帧与之前帧进行特征点匹配,匹配到的点若之前已经估计出深度,就知道了世界坐标系下的三维坐标和当前帧二维点的匹配关系,可以用PNP求解当前帧的相机,相对于世界坐标系的位姿变换。
1. 直接线性变换
用来求 R , t R,t R,t。
待求变量 R , t R,t R,t写成增广矩阵,共有3*4=12个未知量。
变换可得,一对匹配点能够写成两个等式,最少通过6对匹配点,可以求出 R , t R,t R,t。匹配点大于六对时,可以用SVD等方法求最小二乘解。
要注意使得求得的旋转矩阵满足约束。
2 . P3P
SLAM|视觉SLAM十四讲-第七讲笔记
文章图片

**用来求三维点在相机坐标系下的深度。**得到深度后,再用ICP求位姿变换。
仅用三对匹配点,和一个验证点。
已知 A , B , C A,B,C A,B,C在世界坐标系下的三维点坐标, a , b , c a,b,c a,b,c是其在像平面的投影,已知其归一化坐标。
未知 A , B , C A,B,C A,B,C在相机坐标系下的坐标。
根据三角形的相似关系和余弦定理,可以推出:
O A 2 + O B 2 ? 2 O A O B c o s < a , b > = A B 2 OA^2+OB^2-2OAOBcos< a,b> =AB^2 OA2+OB2?2OAOBcos=AB2
O B 2 + O C 2 ? 2 O B O C c o s < b , c > = B C 2 OB^2+OC^2-2OBOCcos< b,c> =BC^2 OB2+OC2?2OBOCcos=BC2
O A 2 + O C 2 ? 2 O A O C c o s < a , c > = A C 2 OA^2+OC^2-2OAOCcos< a,c> =AC^2 OA2+OC2?2OAOCcos=AC2
记 x = O A / O C x=OA/OC x=OA/OC, y = O B / O C y=OB/OC y=OB/OC,得:
x 2 + y 2 ? 2 x y c o s < a , b > = A B 2 / O C 2 x^2+y^2-2xycos< a,b> =AB^2/OC^2 x2+y2?2xycos=AB2/OC2
Y 2 + 1 ? 2 y c o s < b , c > = B C 2 / O C 2 Y^2+1-2ycos< b,c> =BC^2/OC^2 Y2+1?2ycos=BC2/OC2
x 2 + 1 ? 2 x c o s < b , c > = A C 2 / O C 2 x^2+1-2xcos< b,c> =AC^2/OC^2 x2+1?2xcos=AC2/OC2
记 v = A B 2 / O C 2 , U V = B C 2 / O C 2 , w v = A C 2 / O C 2 v=AB^2/OC^2,UV=BC^2/OC^2,wv=AC^2/OC^2 v=AB2/OC2,UV=BC2/OC2,wv=AC2/OC2,得:
( 1 ? u ) y 2 ? u x 2 ? c o s < b , c > y + 2 u x y c o s < a , b > + 1 = 0 (1-u)y^2-ux^2-cos< b,c> y+2uxycos< a,b> +1=0 (1?u)y2?ux2?cosy+2uxycos+1=0
( 1 ? w ) x 2 ? w y 2 ? c o s < a , c > y + 2 w x y c o s < a , b > + 1 = 0 (1-w)x^2-wy^2-cos< a,c> y+2wxycos< a,b> +1=0 (1?w)x2?wy2?cosy+2wxycos+1=0
其中 x , y x,y x,y未知,也就是 A , B , C A,B,C A,B,C在相机坐标系下的坐标 O A , O B , O C OA,OB,OC OA,OB,OC的长度未知。
方程最多得到四个解,用验证点来计算最可能的解,得到3D坐标。之后可以求解两组3D坐标之间的 R , t R,t R,t。
存在问题:
  • 只利用三个点的信息,多于三组点时,难以利用。
  • 存在误匹配或噪声,则失效。
3. bundle adjustment
从非线性优化的角度,一起求解点的深度和相机位姿。
可以用它对PnP和ICP的结果进行优化。
使用李代数表示相机姿态:
s i u i = K e x p ( ? ) ^ P i s_iu_i=Kexp(\epsilon\hat)P_i si?ui?=Kexp(?)^?Pi?
其中, P i P_i Pi?是三维空间点坐标, u i u_i ui?是像素坐标。
等式中存在误差:
  • e x p ( ? ) ^ exp(\epsilon\hat) exp(?)^?的估计误差。
  • u i u_i ui?观测噪声。
定义误差:
e i = u i ? 1 / s i K e x p ( ? ) ^ P i e_i=u_i-1/s_iKexp(\epsilon\hat)P_i ei?=ui??1/si?Kexp(?)^?Pi?
最小化重投影误差:上式定义的误差,是观测值,与估计的位姿把三维点重新投影到像平面上的误差。
? ? = a r g m i n 1 2 ∑ i = 1 n ∣ ∣ u i ? 1 / s i K e x p ( ? ) ^ P i ∣ ∣ 2 2 \epsilon^*=argmin \frac{1}{2}\sum^n_{i=1}||u_i-1/s_iKexp(\epsilon\hat)P_i||^2_2 ??=argmin21?i=1∑n?∣∣ui??1/si?Kexp(?)^?Pi?∣∣22?
最小二乘问题:
  • 变量:位姿的李代数表示: ? \epsilon ?。
  • 目标:最小化重投影误差,求最优的位姿和空间点位姿。
  • 方法:求e关于 ? \epsilon ?或 P P P的导数,进行非线性优化。
    问题转变成如何求导(J矩阵):
    e ( x + δ x ) ≈ e ( x ) + J δ x e(x+\delta x)≈e(x)+J\delta x e(x+δx)≈e(x)+Jδx
    x x x为 ? \epsilon ?或 P P P。
求最优位姿 定义P在相机坐标系下的坐标为 P ′ = e x p ( ? ) ^ P P' =exp(\epsilon\hat)P P′=exp(?)^?P。使用链式法则:
? e ? ? = ? e ? P ′ ? P ′ ? ? \frac{\partial e}{\partial \epsilon} = \frac{\partial e}{\partial P' }\frac{\partial P' }{\partial \epsilon} ???e?=?P′?e????P′?
第一项是2x3的矩阵,第二项是3x6的矩阵,最终J为2x6.
求最优深度 关于 P P P求导。利用链式法则:
? e ? P = ? e ? P ′ ? P ′ ? P \frac{\partial e}{\partial P} = \frac{\partial e}{\partial P' }\frac{\partial P' }{\partial P} ?P?e?=?P′?e??P?P′?
四、3D-3D ICP
  • 已知:两组三维点坐标。(可以是对于同一组特征点,不同相机坐标系下的坐标,也可以是同一相机,拍摄不同位置的空间坐标?)
  • 未知:两个相机坐标系之间的位姿转换。
  • 目标:求变换矩阵,使得匹配点之间的累积距离最小。 p i = R P i ′ + t pi=RP' _i+t pi=RPi′?+t
### 1. SVD方法
误差:
e i = p i ? ( R P i ′ + t ) e_i=pi-(RP' _i+t) ei?=pi?(RPi′?+t)
求使 ∑ i e i \sum_i e_i ∑i?ei?最小的 R , t R,t R,t。
最终目标函数可化简为:
m i n R , t J = 1 2 ∑ i ∣ ∣ p i ? p ? R ( p i ′ ? p ′ ) ∣ ∣ 2 2 + ∣ ∣ p ? R p ′ ? t ∣ ∣ 2 min_{R,t}J=\frac{1}{2} \sum_i ||p_i-p-R(p_i' -p' )||^2_2+||p-Rp' -t||^2 minR,t?J=21?i∑?∣∣pi??p?R(pi′??p′)∣∣22?+∣∣p?Rp′?t∣∣2
先优化第一项,求得使第一项最小的 R R R。
再优化第二项,第二项是平方项,最小为0,即在 R R R的基础上,求使第二项为0的 t t t。
其中 p , p ′ p,p' p,p′是两组点的质心。
2. 非线性优化方法
也是用李代数表达位姿,然后对李代数求导。
ICP问题只有唯一解或无穷多解,在唯一解时,极小值就是全局最优值,即可以任意选择初值。

    推荐阅读