SLAM基础知识总结
1. 视差与深度信息
- 根据双目成像原理,两幅图像摄像机光心位置不同,而且可以看到场景中同一个场景点,则根据此场景点在两幅图像中的**#F00图像坐标(Film Coords/Image Plane)之差(Disparity/Parallax)及两摄像机光心之间的距离(Baseline)**算出场景点的深度信息
- 获取视差图:双目或运动(Stereo or Motion)
- 基本原理:三角形相似原理
2. 3D世界坐标点->2D像素坐标(正向投影)
- 为了进行计算,必须把3D世界坐标点变换到2D像素坐标的过程使用一个数学模型进行描述,最好的描述方式当然是矩阵,它具有平移、旋转、绽放、投影等功能。
- 整个正向投影变换过程如下图所示:
- 整个变换使用
M
e
x
t
M_{ext}
Mext和
M
i
n
t
M_{int}
Mint可以表示为:
[ u v w ] = [ f x 0 o x 0 0 f y o y 0 0 0 1 0 ] [ r 11 r 12 r 13 t x r 21 r 22 r 23 t x r 31 r 32 r 33 t x 0 0 0 1 ] [ U V W 1 ] \begin{bmatrix} u\\ v\\ w\\ \end{bmatrix}= \begin{bmatrix} f_x & 0 & o_x & 0\\ 0 & f_y & o_y & 0\\ 0 & 0 & 1 & 0\\ \end{bmatrix} \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x\\ r_{21} & r_{22} & r_{23} & t_x\\ r_{31} & r_{32} & r_{33} & t_x\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} U\\ V\\ W\\ 1\\ \end{bmatrix} ⎣⎡uvw⎦⎤=⎣⎡fx000fy0oxoy1000⎦⎤⎣⎢⎢⎡r11r21r310r12r22r320r13r23r330txtxtx1⎦⎥⎥⎤⎣⎢⎢⎡UVW1⎦⎥⎥⎤
f x = f / s x f y = f / s y f_x = f/s_x\\ f_y=f/s_y fx=f/sxfy=f/sy - 采有分块矩阵,其表示如下:
[ u v w ] = [ K 3 x 3 0 3 x 1 ] [ R 3 x 3 T 3 x 1 0 1 x 3 1 1 x 1 ] [ P w 3 x 1 1 ] \begin{bmatrix} u\\ v\\ w\\ \end{bmatrix}= \begin{bmatrix} K_{3x3} & 0_{3x1} \\ \end{bmatrix} \begin{bmatrix} R_{3x3} & T_{3x1} \\ 0_{1x3} & 1_{1x1} \\ \end{bmatrix} \begin{bmatrix} P_{w3x1} \\ 1\\ \end{bmatrix} ⎣⎡uvw⎦⎤=[K3x303x1][R3x301x3T3x111x1][Pw3x11]
[ u v w ] = [ K 3 x 3 0 3 x 1 ] [ R P w + T 1 ] = K ( R P w + T ) \begin{bmatrix} u\\ v\\ w\\ \end{bmatrix}= \begin{bmatrix} K_{3x3} & 0_{3x1} \\ \end{bmatrix} \begin{bmatrix} RP_w+T \\ 1 \\ \end{bmatrix}= K(RP_w+T) ⎣⎡uvw⎦⎤=[K3x303x1][RPw+T1]=K(RPw+T)
2.1 世界坐标 → \to →摄像机坐标( M e x t M_{ext} Mext)
- 在实际问题中, 场景点使用世界坐标进行描述,这样符合我们对环境的认知,因为环境是相对不变,而摄像机总是不停地运动。
- 基本原理:刚体变换=平移(T)+旋转®
- T: 此T不等于摄像机光心在世界坐标系中的位置,具体值见下面的公式
2.2 摄像机坐标 → \to →图像坐标( M p r o j M_{proj} Mproj)
- 摄像机坐标
→
\to
→图像坐标是3D到2D的透视投影变换
2.3 图像坐标 → \to →像素坐标( M a f f M_{aff} Maff)
2.3.1 内部参数-偏移量(Intrinsic Parameters-Offsets)
- 坐标变换:描述图像坐标与像素坐标间的坐标变换
- 数字图像:把图像坐标系中的图像经过扫描、数字化、重采样便生成了像素坐标图像
- 内部参数: 偏移量(Offset)
2.3.2 内部参数-尺度变换(Intrinsic parameters -scales)
- 内部参数:尺度变换(Scale: 即宽高比被改变了)
- 数字图像行/列数:由采样决定
2.3.3 仿射变换 (Affine Transformation)
- 仿射变换:内部参数-偏移量与内部参数尺度变换的统称,它是由6个参数定义的2D$\to$2D的变换,其变换矩阵如下图所示
M a f f = [ 1 / s x 0 o x 0 1 / s y o y 0 0 1 ] M_{aff}=\begin{bmatrix} 1/s_x & 0 & o_x\\ 0 & 1/s_y & o_y\\ 0 & 0 & 1\\ \end{bmatrix} Maff=⎣⎡1/sx0001/sy0oxoy1⎦⎤
2.4 内部参数变换 (M_{int})
- M_{ext}: 是由摄像机外部参数(R & T)决定的变换矩阵,所以称为外部参数变换矩阵
- M_{int}: 是由摄像机内部参数【f( f x = f / s x , f y = f / s y f_x=f/s_x, f_y=f/s_y fx=f/sx,fy=f/sy), Offsets( o x , o y o_x, o_y ox,oy), Scales( s x , s y s_x, s_y sx,sy) 】决定的,所以称为内部参数变换矩阵
- 内部参数变换矩阵:是透视投影变换与仿射变换的组合
2.5 3D世界到2D图像的总结
- x = P X x=PX x=PX: x x x为2D数字图像点(2D image point), P P P为摄像机矩阵(Camera Matrix), X X X为3D世界场景点(3D World Point)
- 没有平移和旋转的情况, 如下图所示:
- 有平移和旋转的情况,如下图所示:
3. 共面点成像(单应性矩阵:Homography)
3.1 单应矩阵定义
- x ′ = H x x'=Hx x′=Hx
3.2 目的
- 研究共面点成像的目的:为了研究两幅图之间的关系,其关系为: x ′ = H x x'=Hx x′=Hx
- 在知道了A、B图像之间的单应矩阵之后,就可以根据A中一点计算出在B中对应的点
- 共面点成像:即所有的场景点都处在同一个平面上,这是通用透视投影成像的特殊情况
- 可以3D$\to 2 D 的 变 换 简 化 为 2 D 2D的变换简化为2D 2D的变换简化为2D\to$2D的变换,且此变换是可逆的,这对求场景点的3D坐标非常方便
3.3 场景点共面的成像矩阵
3.4共面点组成的平面垂直光轴(摄像机坐标Z轴)
- 当共面点组成的平面垂直光轴时,此变换可以进一步简化
3.5 单应矩阵的用途
-
如果x和x’分别是场景点 P w P_w Pw在image1和image2上的像素点,则可以得知x’与x的变换关系
-
在下面的计算中, H 1 和 H 2 H_1和H_2 H1和H2可逆是关键,非共面点的投影可没有这种特殊待遇
x = [ u v 1 ] x ′ = [ u ′ v ′ 1 ] P w = [ p q 1 ] x = H 1 P w ⇒ H 1 − 1 x = P w x ′ = H 2 P w ⇒ H 2 − 1 x ′ = P w H 1 − 1 x = H 2 − 1 x ′ ⇒ x ′ = H 2 H 1 − 1 x ⇒ x ′ = H x x=\begin{bmatrix} u\\ v\\ 1\\ \end{bmatrix} x'=\begin{bmatrix} u'\\ v'\\ 1\\ \end{bmatrix} P_w=\begin{bmatrix} p\\ q\\ 1\\ \end{bmatrix}\\ x = H_1P_w\Rightarrow H_1^{-1}x=P_w\\ x' = H_2P_w\Rightarrow H_2^{-1}x'=P_w\\ H_1^{-1}x=H_2^{-1}x'\Rightarrow x'=H_2H_1^{-1}x\Rightarrow\\ x'=Hx x=⎣⎡uv1⎦⎤x′=⎣⎡u′v′1⎦⎤Pw=⎣⎡pq1⎦⎤x=H1Pw⇒H1−1x=Pwx′=H2Pw⇒H2−1x′=PwH1−1x=H2−1x′⇒x′=H2H1−1x⇒x′=Hx -
根据Image1(x)Image2(x’)图像对应点对(可通过ORB匹配求出),则可以求出H
-
求出H之后,就可以求出 P w P_w Pw??
4. 对极几何(Epipolar Geometry)
4.1目标
- 描述两张照片间的对应关系:用两个相机在不同的位置拍摄同一物体,如果两张照片中的景物有重叠的部分, 则这两张照片之间存在一定的对应关系
- 描述工具:对极几何(Epipolar Geometry),它是美丽的对极几何,可用于描述多视图间的几何关系
- 对何几何的用途: 寻找两幅图像之间的对应关系,最直接的方法就是逐点匹配,如果加以一定的约束条件对极约束(epipolar constraint),搜索的范围可以大大减小,且减少到一条线
- 通用的双目成像:本文前面描述的双目成像和共面点成像,对摄像机位姿和内部参数都有要求,研究起来相对简单,现在开始研究通用双目成像,需要使用对极矩阵和基本矩阵来描述:
- 对极矩阵:两个摄像机的位姿无任何约束关系(即任意R & T),需要使用对极矩阵(Epipolar Matrix)来描述
- 基本矩阵:两个摄像机内部参数不同或未,需要使用基本矩阵(Fundamental Matrix)来描述
4.2 对极几何关键概念
-
极点Epipoles:
-
极点 e l e_l el:右相机光心 O r O_r Or在左像平面上的像
-
极点 e r e_r er:左相机光心 O l O_l Ol在右像平面上的像
-
极平面(Epipolar Plane):由左右光心 O l , O r O_l, O_r Ol,Or, 场景点P三点确定的平面
-
极线(Epipolar Line):极平面与左右像平面的交线,如 p l e l p_le_l plel和 p r e r p_re_r prer
-
极线约束(Epipolar Constraint):对应点在共轭极线上(如 p l e l p_le_l plel和 p r e r p_re_r prer)
-极线约束的用途:已知场景点P在左图的像点 p l p_l pl,则其在右图的像点一定在对应的极线 p r e r p_re_r prer上,这大大减少了 p l p_l pl在右图中寻找匹配点的搜索范围,减少为区区一条线
5. 本质矩阵(Essential Matrix)摄像机坐标
- 本质矩阵定义:在摄像机坐标系中,描述两幅视图(为了方便, 经常描述为左右图,其实只要是任意两幅有重叠部分的图像即可)中的物点矢量间的约束关系
- 本质矩阵的用途:大大减少匹配搜索空间,即缩减为在一条线上进行搜索
- 摄像机
→
\rightarrow
→摄像机的变换类似世界坐标
→
\rightarrow
→摄像机坐标的变换
5.1 物点矢量( P l , P r P_l, P_r Pl,Pr)间的本质矩阵(E)-摄像机坐标
- 根据向量的减法:
P r ′ P'_r Pr′ = P l P_l Pl-T - 把向量
P
r
′
P'_r
Pr′进行旋转,可得:
P r = R P r ′ = R ( P l − T ) P_r = RP'_r = R(P_l-T) Pr=RPr′=R(Pl−T) - R是正交矩阵,则R可逆:
P l − T = R − 1 P r = R T P r P_l-T = R^{-1}P_r = R^TP_r Pl−T=R−1Pr=RTPr - 三向量(
P
l
,
T
,
P
l
−
T
P_l, T, P_l-T
Pl,T,Pl−T)共面,它们的混合积为零(混合积对应于有向体积):
( P l − T ) T ⋅ T × P l = 0 ⇒ ( R T P r ) T ⋅ T × P l = 0 ⇒ (P_l-T)^T{\cdot}T{\times}P_l = 0 \Rightarrow (R^TP_r)^T \cdot T \times P_l = 0 \Rightarrow (Pl−T)T⋅T×Pl=0⇒(RTPr)T⋅T×Pl=0⇒
P r T R ⋅ T × P l = 0 P_r^TR \cdot T \times P_l = 0 PrTR⋅T×Pl=0 - 把向量表示为把对称矩阵的形式,从而把叉乘可以写为矩阵的形式:
- 本质矩阵的由来
P r T R ⋅ T × P l = 0 ⇒ P_r^TR \cdot T \times P_l = 0 \Rightarrow PrTR⋅T×Pl=0⇒
P r T R ⋅ S P l = 0 ⇒ P_r^TR \cdot SP_l = 0 \Rightarrow PrTR⋅SPl=0⇒
P r T R S P l = 0 ⇒ P_r^TRSP_l = 0 \Rightarrow PrTRSPl=0⇒
$P_r^TEP_l = 0 $
上式中的E即为本质矩阵,它等于RS(S是向量T的对称矩阵表示形式),即E只依赖摄像机外部参数(R & T) - 本质矩阵的属性:
- 它的秩为2
- 它只依赖摄像机外部参数(R & T)
- 它定义了在摄像机坐标系中,左右视图之间**物点矢量( P l , P r P_l, P_r Pl,Pr)之间的约束关系,以及像点矢量( p l , p r p_l, p_r pl,pr)**间的约束关系
- 本质矩阵(E)定义的左右图约束关系是什么?
- P r P_r Pr位于 E P l EP_l EPl定义的在右图的极线上,左视图上的一点( P l P_l Pl)乘本质矩阵( E E E)的结果为 P l P_l Pl在右图上对应的极线
- P l P_l Pl位于 E T P r E^TP_r ETPr定义的在左图的极线上,本质矩阵( E T E^T ET)乘右视图上的一点( P r P_r Pr)的结果为 P r P_r Pr在左图上对应的极线
-现在明白极线的用途了吧!知道左图上的一点,就可以找到其在右图上对应的极线,如果要搜索左图上此点在右图上的匹配点,则在此对应的极线上搜索即可
5.2 像点矢量( p l , p r p_l,p_r pl,pr)间的本质矩阵(E)-摄像机坐标
- 在摄像机坐标系中,描述像点矢量
p
l
,
p
r
p_l,p_r
pl,pr间的约束关系 (下面的等式中,
Z
r
,
f
r
,
Z
l
,
f
l
Z_r, f_r, Z_l, f_l
Zr,fr,Zl,fl均为常数)
- 在图像平面中的点可以被看作为观察射线,下面的描述是等价的:
• (u,v) 2D film point (图像平面上的2D点)
• (u,v,f) 3D point on film plane (图像平面上的3D点)
• k(u,v,f) viewing ray into the scene (看场景的观察射线)
• k(X, Y, Z) ray through point P in the scene [设: k=f/Z, 则:u=fX/Z, v=fY/Z]. (在场景中通过点P的射线) - 极线方程
- 总结:物点矢量间的本质矩阵与像点矢量间的本质矩阵相等 ,即摄像机坐标系下的本质矩阵(E)只有一个。
5.3 本质矩阵-对极几何描述
- 基本原理:三个矢量(x, x’, T)共面
- 点x和x’位于归一化图像坐标:归一化图像坐标的原点在图像的光心,x轴被
f
x
f_x
fx归一化,y轴被
f
y
f_y
fy归一化,因此它是没有尺寸概念的
- 本质矩阵(E)的属性
6. 基本矩阵 (Fundamental Matrix)像素坐标
- 由于本质矩阵只能描述摄像机坐标系下两个视图间的约束关系,所以它只依赖摄像机的外部参数(R & T)
- 如果需要描述像素坐标系下两个图像间的约束关系,它必须在基本矩阵的基础上再进行仿射变换,即依赖内部参数(
f
x
=
f
/
s
x
,
f
y
=
f
/
s
y
,
o
x
,
o
y
f_x=f/s_x, f_y=f/s_y, o_x, o_y
fx=f/sx,fy=f/sy,ox,oy)和外部参数(R & T)
- 基本矩阵定义: F = M r − T E M l − 1 F = M_r^{-T}EM_l^{-1} F=Mr−TEMl−1
- 基本矩阵的特性:
- 它的秩为2
- 它依赖内部参数( f x = f / s x , f y = f / s y , o x , o y f_x=f/s_x, f_y=f/s_y, o_x, o_y fx=f/sx,fy=f/sy,ox,oy)和外部参数 (R & T)
6.1 基本矩阵-对极几何描述
-
Camera resectioning:是估计一个针孔相机模型的参数的过程,针孔相机参数为3x4的矩阵,叫做Camera Matrix。这个过程叫做Camera Calibration。
-
R T R \quad T RT (摄像机外部参数): 表示从3D世界坐标系到3D摄像机坐标系的坐标系统变换,同时,它也定义了在世界坐标系中,摄像机光心的位置和摄像机的方向
-
T T T (世界坐标系原点的位置):它表示世界坐标系原点在摄像机坐标系中的位置;它经常被误认为是摄像机的位置
-
C C C (摄像机位置):它表示摄像机光心在世界坐标系中的位置。
$T=-RC \Rightarrow C=-R{-1}T=-RTT $ (R是旋转矩阵,即行列式为+1的正交矩阵) -
公式推导过程:
x ˉ = K x x ˉ ′ = K ′ x ′ ⇒ {\bar x}=Kx \quad {\bar x'=K'x' \quad } \Rightarrow xˉ=Kxxˉ′=K′x′⇒
x = K − 1 x ˉ x ′ = K ′ − 1 x ˉ ′ ⇒ x=K^{-1} \bar x \quad x'=K'^{-1} \bar x' \Rightarrow x=K−1xˉx′=K′−1xˉ′⇒$ {x’^TEx = 0} \Rightarrow$
x ˉ ′ T K ′ − T E K − 1 x ˉ = 0 ⇒ \bar x'^TK'^{-T}EK^{-1} \bar x = 0 \Rightarrow xˉ′TK′−TEK−1xˉ=0⇒
x ˉ ′ T F x ˉ = 0 ⇒ \bar x'^TF\bar x = 0 \Rightarrow xˉ′TFxˉ=0⇒
F = K ′ − T E K − 1 ⇒ F= K'^{-T}EK^{-1} \Rightarrow F=K′−TEK−1⇒
E = K ′ T F K E=K'^{T}FK E=K′TFK
-
基本矩阵(F)的属性
7. 三个矩阵对比分析(单应+本质+基本)
- F是E的推广:把假设"校准的摄像机"去掉了
- x {x} x:图像坐标系中的点
- x ˉ {\bar x} xˉ:像素坐标系中的点
- 本质矩阵(E)+基本矩阵(F):用于描述2个视图间的对极几何关系
- 变换种类:(1)透视变换:world → \rightarrow →camera (2) 射影变换:image → \rightarrow →image
- points aligned to camera coordinate axis
| 矩阵属性 | 本质矩阵 (Essential) | 基础矩阵 (Fundamental) | 单应矩阵 (Homography) |
|:-------|:----------|:-----|:-----|
|用途 | 把图像点映射到极线 | 把像素点映射到极线) | 把像素点映射到像素点 |
|矩阵定义 | E = R S = R [ t × ] E=RS=R[t_\times] E=RS=R[t×] | F = K ′ − T E K − 1 F=K'^{-T}EK^{-1} F=K′−TEK−1 | |
|相互关系 | E = K ′ T F K E=K'^TFK E=K′TFK | F = K ′ − T E K − 1 F=K'^{-T}EK^{-1} F=K′−TEK−1 | |
|映射方程 | l ′ = E x l'=Ex l′=Ex | l ˉ ′ = F x ˉ {\bar l}'=F{\bar x} lˉ′=Fxˉ | x ˉ ′ = H x ˉ {\bar x}'=H{\bar x} xˉ′=Hxˉ |
|映射关系 | x ′ T E x = 0 x'^TEx = 0 x′TEx=0 | x ˉ ′ T F x ˉ = 0 {\bar x}'^TF{\bar x} = 0 xˉ′TFxˉ=0 | x ˉ ′ = H x ˉ {\bar x}'=H{\bar x} xˉ′=Hxˉ |
|坐标系 |图像坐标系(归一化的) |像素坐标系|像素坐标系|
|摄像机 |校准摄像机 |未校准摄像机|未校准摄像机|
|矩阵维数 | 3 × 3 3\times3 3×3 | 3 × 3 3\times3 3×3 | 3 × 3 3\times3 3×3 |
|矩阵的秩 |2(不可逆) |2(不可逆) |3(可逆) |
|矩阵自由度 |5 | 7 | 8 |
|依赖参数 | 外参(R&T) | 内参( f x , f y , c x , c y f_x,f_y,c_x,c_y fx,fy,cx,cy)和外参(R&T) | ? |
|场景结构 |独立于场景结构(即任意场景) |独立于场景结构(即任意场景) | 场景点在同一平面或离相机很远,是基本矩阵的特殊情况 |
|计算方法1|仅由两幅图的匹配点计算(5点算法) |仅由两幅图的匹配点计算(8点算法) |仅由两幅图的匹配点计算 |
|用途|可求出R & T |可求出E,但必须已知内参K| |
| | | | |
参考:
- CSE/EE 486: Computer Vision I Fall 2007 Course Overview
- 16-385 Computer Vision Carnegie Mellon University (Kris Kitani)