2012年就接触了三维视觉和SLAM,monoSLAM,基于EKF的。当时这个方向还没有起来,现在在ARVR、机器人、自动驾驶中都需SLAM技术。疫情出不去,居家办公之余,决定重温一下三维视觉和SLAM这块的知识。
网上看了北邮鲁鹏老师的课程《计算机视觉之三维重建(深入浅出sfm和SLAM核心算法)》 ,讲的非常好,适合入门和温顾。三维视觉基础部分讲的很细,最后用分别用OpenMVG和ORB-SLAM框架来讲SFM和SLAM。这边整理一下学这门课的笔记(如果之前能够做笔记,现在也不至于网上到处找资料温习,十年换来的教训啊,以后要及时做笔记)
1,数学基础
注:这边的基础是建立在最基本的线性代数上的,矩阵、向量、行列式、秩、单位矩阵、SVD分解等等基本的概念是本节的前提。
1.1 最小二乘
1.1.1 线性方程组
问题:求解线性方程组
A
x
=
y
Ax=y
Ax=y
其中
A
=
(
a
11
…
a
1
q
⋮
⋱
⋮
a
p
1
…
a
p
q
)
x
=
(
x
1
⋮
x
q
)
y
=
(
y
1
⋮
y
p
)
A=\left( \begin{array}{c} a_{11}&\ldots&a_{1q} \\ \vdots&\ddots&\vdots \\ a_{p1}&\ldots&a_{pq} \end{array}\right) \ x=\left(\begin{array}{l}x_1\\ \vdots\\ x_q\end{array}\right) \ y=\left(\begin{array}{l}y_1\\ \vdots\\ y_p\end{array}\right)
A=⎝⎜⎛a11⋮ap1…⋱…a1q⋮apq⎠⎟⎞ x=⎝⎜⎛x1⋮xq⎠⎟⎞ y=⎝⎜⎛y1⋮yp⎠⎟⎞
A
\ A
A列满秩且
p
>
q
p>q
p>q
方程无解,要求解的是 x ∗ = a r g min x ∣ ∣ A x − y ∣ ∣ 2 x^*=arg\min_x||Ax-y||^2 x∗=argminx∣∣Ax−y∣∣2
解法1
x ∗ = ( A T A ) − 1 A T y x^*=(A^TA)^{-1}A^Ty x∗=(ATA)−1ATy
证明:
f ( x ) = ∣ ∣ A x − y ∣ ∣ 2 = x T A T A x − 2 y T A x + y T y f(x)=||Ax-y||^2=x^TA^TAx-2y^TAx+y^Ty f(x)=∣∣Ax−y∣∣2=xTATAx−2yTAx+yTy
最小值处偏导为0: ∂ f ∂ x ∣ x ∗ = 2 A T A x ∗ − 2 A T y = 0 \frac{\partial{f}}{\partial{x}}|_{x^*}=2A^TAx^*-2A^Ty=0 ∂x∂f∣x∗=2ATAx∗−2ATy=0
⇒ x ∗ = ( A T A ) − 1 A T y \Rightarrow x^*=(A^TA)^{-1}A^Ty ⇒x∗=(ATA)−1ATy
注: 若 F ( x ) = A x , 则 ∂ F ∂ x = A T , ∂ F ∂ x 定 义 为 ( ∂ F 1 ∂ x 1 … ∂ F p ∂ x 1 ⋮ ⋱ ⋮ ∂ F 1 ∂ x q … ∂ F p ∂ x q ) 若F(x)=Ax,则\frac{\partial{F}}{\partial{x}}=A^T,\frac{\partial{F}}{\partial{x}}定义为\left( \begin{array}{c} \frac{\partial{F_1}}{\partial{x_1}}&\ldots&\frac{\partial{F_p}}{\partial{x_1}} \\ \vdots&\ddots&\vdots \\ \frac{\partial{F_1}}{\partial{x_q}}&\ldots&\frac{\partial{F_p}}{\partial{x_q}} \end{array}\right) 若F(x)=Ax,则∂x∂F=AT,∂x∂F定义为⎝⎜⎜⎛∂x1∂F1⋮∂xq∂F1…⋱…∂x1∂Fp⋮∂xq∂Fp⎠⎟⎟⎞
解法2
x
∗
=
V
b
x^*=Vb
x∗=Vb
其中:奇异值分解
A
=
U
D
V
T
A=UDV^T
A=UDVT,
x
ˉ
=
V
T
x
,
y
ˉ
=
U
T
y
,
b
i
=
y
i
ˉ
/
d
i
,
d
i
=
D
i
i
\bar{x}=V^Tx,\ \bar{y}=U^Ty,\ b_i=\bar{y_i}/d_i,\ d_i=D_{ii}
xˉ=VTx, yˉ=UTy, bi=yiˉ/di, di=Dii为对角矩阵
D
D
D的对角
证明:
f ( x ) = ∣ ∣ A x − y ∣ ∣ 2 = ∣ ∣ U D V T x − U U T y ∣ ∣ 2 = ∣ ∣ U ∣ ∣ 2 ∣ ∣ D x ˉ − y ˉ ∣ ∣ 2 = ∑ i = 1 q ( d i x ˉ i − y ˉ i ) 2 + ∑ i = q + 1 p y ˉ i 2 f(x)=||Ax-y||^2\\=||UDV^Tx-UU^Ty||^2\\=||U||^2||D\bar{x}-\bar{y}||^2\\=\sum_{i=1}^{q}(d_i\bar{x}_i-\bar{y}_i)^2+\sum_{i=q+1}^{p}\bar{y}_i^2 f(x)=∣∣Ax−y∣∣2=∣∣UDVTx−UUTy∣∣2=∣∣U∣∣2∣∣Dxˉ−yˉ∣∣2=∑i=1q(dixˉi−yˉi)2+∑i=q+1pyˉi2
求最小值,则: x ˉ i = y ˉ i / d i = b i \bar{x}_i=\bar{y}_i/d_i=b_i xˉi=yˉi/di=bi,即 x ˉ = b \bar{x}=b xˉ=b,所以 V T x = b V^Tx=b VTx=b
得到: x = V b x=Vb x=Vb
解法3
无约束优化问题,用梯度下降、牛顿法、LM求解
1.1.2 齐次线性方程组
A
x
=
0
Ax=0
Ax=0
方程无非0解,要求解的是约束线性优化问题:
x
∗
=
a
r
g
min
x
∣
∣
A
x
∣
∣
2
s
.
t
.
∣
∣
x
∣
∣
2
=
1
x^*=arg\min_x||Ax||^2\\s.t.||x||^2=1
x∗=argxmin∣∣Ax∣∣2s.t.∣∣x∣∣2=1
解法
x ∗ = V ( 0 ⋮ 0 1 ) x^*=V\left(\begin{array}{l}0\\ \vdots\\0\\1\end{array}\right) x∗=V⎝⎜⎜⎜⎛0⋮01⎠⎟⎟⎟⎞为 V V V的最后一列
证明:
f ( x ) = ∣ ∣ A x ∣ ∣ 2 = ∣ ∣ U D V T x ∣ ∣ 2 = ∣ ∣ D x ˉ ∣ ∣ 2 = ∑ i = 1 q ( d i x ˉ i ) 2 = ( d 1 2 , ⋯ , d q 2 ) ( x ˉ 1 2 ⋮ x ˉ q 2 ) f(x)=||Ax||^2\\=||UDV^Tx||^2\\=||D\bar{x}||^2\\=\sum_{i=1}^{q}(d_i\bar{x}_i)^2\\=(d_1^2,\cdots,d_q^2) \left(\begin{array}{l}\bar{x}_1^2\\\vdots\\\bar{x}_q^2\end{array}\right) f(x)=∣∣Ax∣∣2=∣∣UDVTx∣∣2=∣∣Dxˉ∣∣2=∑i=1q(dixˉi)2=(d12,⋯,dq2)⎝⎜⎛xˉ12⋮xˉq2⎠⎟⎞
因为 ∣ ∣ x ∣ ∣ 2 = 1 ||x||^2=1 ∣∣x∣∣2=1,所以 ∣ ∣ x ˉ ∣ ∣ 2 = 1 ||\bar{x}||^2=1 ∣∣xˉ∣∣2=1
所以,要使 f ( x ) f(x) f(x)最小,则 x ˉ = ( 0 ⋮ 0 1 ) \bar{x}=\left(\begin{array}{l}0\\ \vdots\\0\\1\end{array}\right) xˉ=⎝⎜⎜⎜⎛0⋮01⎠⎟⎟⎟⎞,进而得到 x ∗ x^* x∗
1.1.3 非线性方程组
{
f
1
(
x
1
,
⋯
,
x
q
)
=
0
f
2
(
x
1
,
⋯
,
x
q
)
=
0
⋮
f
p
(
x
1
,
⋯
,
x
q
)
=
0
\left\{\begin{array}{c}f_1(x_1,\cdots,x_q)=0\\ f_2(x_1,\cdots,x_q)=0\\ \vdots\\ f_p(x_1,\cdots,x_q)=0 \end{array}\right.
⎩⎪⎪⎪⎨⎪⎪⎪⎧f1(x1,⋯,xq)=0f2(x1,⋯,xq)=0⋮fp(x1,⋯,xq)=0
转化为优化问题:
x
∗
=
a
r
g
min
x
1
2
∑
i
∣
∣
f
i
(
x
)
∣
∣
2
x^*=arg\min_x\frac{1}{2}\sum_i||f_i(x)||^2
x∗=argminx21∑i∣∣fi(x)∣∣2,用梯度下降、牛顿法、LM求解
1.2 牛顿法与LM法
问题:求
x
∗
=
arg min
x
f
(
x
)
x^*=\argmin_xf(x)
x∗=xargminf(x) 对
f
(
x
)
f(x)
f(x)进行泰勒展开:
f
(
x
)
=
f
(
x
0
)
+
f
′
(
x
0
)
(
x
−
x
0
)
+
1
2
f
′
′
(
x
0
)
(
x
−
x
0
)
2
+
⋯
f(x)=f(x_0)+f'(x_0)(x-x_0)+\frac{1}{2}f''(x_0)(x-x_0)^2+\cdots
f(x)=f(x0)+f′(x0)(x−x0)+21f′′(x0)(x−x0)2+⋯
梯度下降
x n + 1 = x n − α f ′ ( x n ) x_{n+1}=x_n-\alpha f'(x_n) xn+1=xn−αf′(xn)
证明
用泰勒展开的一阶导数来近似:
f ( x n + 1 ) = f ( x n ) + f ′ ( x n ) ( x n + 1 − x n ) = f ( x n ) − α ( f ′ ( x n ) ) 2 f(x_{n+1})=f(x_n)+f'(x_n)(x_{n+1}-x_n)=f(x_n)-\alpha(f'(x_n))^2 f(xn+1)=f(xn)+f′(xn)(xn+1−xn)=f(xn)−α(f′(xn))2
当 x n x_n xn不是最小值时 f ′ ( x n ) ≠ 0 f'(x_n)\ne0 f′(xn)=0,所以 f ( x n + 1 ) < f ( x n ) f(x_{n+1})<f(x_n) f(xn+1)<f(xn),在逼近最优值 x ∗ x^* x∗。
牛顿法
用泰勒展开的二阶导数来近似: f ( x ) = f ( x 0 ) + f ′ ( x 0 ) ( x − x 0 ) + 1 2 f ′ ′ ( x 0 ) ( x − x 0 ) 2 f(x)=f(x_0)+f'(x_0)(x-x_0)+\frac{1}{2}f''(x_0)(x-x_0)^2 f(x)=f(x0)+f′(x0)(x−x0)+21f′′(x0)(x−x0)2求导得: f ′ ( x ) = f ′ ( x 0 ) + f ′ ′ ( x 0 ) ( x − x 0 ) f'(x)=f'(x_0)+f''(x_0)(x-x_0) f′(x)=f′(x0)+f′′(x0)(x−x0)最小值处导数为 f ′ ( x ) = 0 f'(x)=0 f′(x)=0: x = x 0 − ( f ′ ′ ( x 0 ) ) − 1 f ′ ( x 0 ) x=x_0-(f''(x_0))^{-1}f'(x_0) x=x0−(f′′(x0))−1f′(x0)如果 x x x是多维向量,则 f ′ ′ ( x ) f''(x) f′′(x)为 f ( x ) f(x) f(x)的Hessian矩阵 H ( x ) = ( ∂ 2 f ∂ 2 x 1 ⋯ ∂ 2 f ∂ x 1 ∂ x n ⋮ ⋱ ⋮ ∂ 2 f ∂ x n ∂ x 1 ⋯ ∂ 2 f ∂ 2 x n ) H(x)=\left(\begin{array}{c} \frac{\partial^2f}{\partial^2 x_1}&\cdots&\frac{\partial^2f}{\partial x_1\partial x_n}\\ \vdots&\ddots&\vdots\\ \frac{\partial^2f}{\partial x_n\partial x_1}&\cdots&\frac{\partial^2f}{\partial^2 x_n} \end{array}\right) H(x)=⎝⎜⎜⎛∂2x1∂2f⋮∂xn∂x1∂2f⋯⋱⋯∂x1∂xn∂2f⋮∂2xn∂2f⎠⎟⎟⎞所以,牛顿法的迭代公式为: x n + 1 = x n − H − 1 f ′ ( x n ) x_{n+1}=x_n-H^{-1} f'(x_n) xn+1=xn−H−1f′(xn)
高斯牛顿法
牛顿法中 H H H矩阵要求二阶导数,实际中很难操作,用雅可比矩阵来代替 H ≈ J T J H\approx J^TJ H≈JTJ: x n + 1 = x n − ( J T J ) − 1 f ′ ( x n ) x_{n+1}=x_n-(J^TJ)^{-1} f'(x_n) xn+1=xn−(JTJ)−1f′(xn) 这边的 J ( x ) = ▽ f ( x ) = f ′ ( x ) = ( ∂ f ∂ x 1 , ⋯ , ∂ f ∂ x n ) T J(x)=\triangledown f(x)=f'(x)=(\frac{\partial f}{\partial x_1},\cdots,\frac{\partial f}{\partial x_n})^T J(x)=▽f(x)=f′(x)=(∂x1∂f,⋯,∂xn∂f)T
高斯牛顿这边的近似 H H H矩阵不满秩,无法求逆,其实高斯牛顿法不是这样用的,常用在最小二乘问题中,把这边的 f f f变成多个方程的 f i f_i fi,雅可比矩阵 J J J就能是满秩了。
LM
(Levenberg-Marquadt)加一个缩放的单位矩阵
μ
I
\mu I
μI,防止
J
T
J
J^TJ
JTJ不满秩:
x
n
+
1
=
x
n
−
(
J
T
J
+
μ
I
)
−
1
f
′
(
x
n
)
x_{n+1}=x_n-(J^TJ+\mu I)^{-1} f'(x_n)
xn+1=xn−(JTJ+μI)−1f′(xn) 相当于梯度下降和高斯牛顿的混合。在最小二乘问题中,同高斯牛顿法,这边的
J
J
J可以换成向量函数
(
f
i
)
(f_i)
(fi)对
x
x
x的雅可比矩阵。
1.3 变换
1.3.1 2D变换
x = ( x y 1 ) , x ′ = ( x ′ y ′ 1 ) \boldsymbol{x}=\left(\begin{array}{c}x\\y\\1\end{array}\right),\boldsymbol{x'}=\left(\begin{array}{c}x'\\y'\\1\end{array}\right) x=⎝⎛xy1⎠⎞,x′=⎝⎛x′y′1⎠⎞
- 欧式变换
x ′ = ( R t 0 1 ) x \boldsymbol{x'}=\left(\begin{array}{c}R&t\\0&1\end{array}\right)\boldsymbol{x} x′=(R0t1)x R = ( cos θ − sin θ sin θ cos θ ) R=\left(\begin{array}{c} \cos\theta&-\sin\theta\\ \sin\theta&\cos\theta\\ \end{array}\right) R=(cosθsinθ−sinθcosθ) - 相似变换
x ′ = ( s R t 0 1 ) x \boldsymbol{x'}=\left(\begin{array}{c}sR&t\\0&1\end{array}\right)\boldsymbol{x} x′=(sR0t1)x - 仿射变换
x ′ = ( A t 0 1 ) x \boldsymbol{x'}=\left(\begin{array}{c}A&t\\0&1\end{array}\right)\boldsymbol{x} x′=(A0t1)x - 透视变换
x ′ = ( A t v T 1 ) x \boldsymbol{x'}=\left(\begin{array}{c}A&t\\v^T&1\end{array}\right)\boldsymbol{x} x′=(AvTt1)x
1.3.2 3D变换
变换公式形同2D变换,维度由2维拓展到3维即可。
1.3.3 3D欧式变换
三维空间的欧式变换(正交变换),有两种几何意义(表示):
欧拉表示法
分别围绕
X
,
Y
,
Z
X,Y,Z
X,Y,Z轴的旋转
α
,
β
,
γ
\alpha,\beta,\gamma
α,β,γ角度,可以得到欧式变换:
R
=
R
x
(
α
)
R
y
(
β
)
R
z
(
γ
)
R=R_x(\alpha)R_y(\beta)R_z(\gamma)
R=Rx(α)Ry(β)Rz(γ) 其中
R
x
(
α
)
=
(
1
0
0
0
cos
α
−
sin
α
0
sin
α
cos
α
)
R_x(\alpha)=\left(\begin{array}{c} 1&0&0\\ 0&\cos\alpha&-\sin\alpha\\ 0&\sin\alpha&\cos\alpha\\ \end{array}\right)
Rx(α)=⎝⎛1000cosαsinα0−sinαcosα⎠⎞
R
y
(
β
)
=
(
cos
β
0
sin
β
0
1
0
−
sin
β
0
cos
β
)
R_y(\beta)=\left(\begin{array}{c} \cos\beta&0&\sin\beta\\ 0&1&0\\ -\sin\beta&0&\cos\beta\\ \end{array}\right)
Ry(β)=⎝⎛cosβ0−sinβ010sinβ0cosβ⎠⎞
R
z
(
γ
)
=
(
cos
γ
−
sin
γ
0
sin
γ
cos
γ
0
0
0
1
)
R_z(\gamma)=\left(\begin{array}{c} \cos\gamma&-\sin\gamma&0\\ \sin\gamma&\cos\gamma&0\\ 0&0&1 \end{array}\right)
Rz(γ)=⎝⎛cosγsinγ0−sinγcosγ0001⎠⎞
四元数
可以围绕单位向量
u
=
(
u
x
,
u
y
,
u
z
)
u=(u_x,u_y,u_z)
u=(ux,uy,uz)旋转
θ
\theta
θ角度,表示为
R
u
(
θ
)
R_u(\theta)
Ru(θ)
用四元数表示:
q
=
(
x
,
y
,
z
,
w
)
=
(
u
x
sin
(
θ
/
2
)
,
u
y
sin
(
θ
/
2
)
,
u
z
sin
(
θ
/
2
)
,
cos
(
θ
/
2
)
)
q=(x,y,z,w)=(u_x\sin(\theta/2),u_y\sin(\theta/2),u_z\sin(\theta/2),\cos(\theta/2))
q=(x,y,z,w)=(uxsin(θ/2),uysin(θ/2),uzsin(θ/2),cos(θ/2)) 则欧式变换可表示为:
R
=
R
u
(
θ
)
=
R
q
=
(
1
−
2
y
2
−
2
z
2
2
x
y
−
2
w
z
2
x
z
+
2
w
y
2
x
y
+
2
w
z
1
−
2
x
2
−
2
z
2
2
y
z
−
2
w
x
2
x
z
−
2
w
y
2
y
z
+
2
w
x
1
−
2
x
2
−
2
y
2
)
R=R_u(\theta)=R_q= \left(\begin{array}{c} 1-2y^2-2z^2&2xy-2wz&2xz+2wy\\ 2xy+2wz&1-2x^2-2z^2&2yz-2wx\\ 2xz-2wy&2yz+2wx&1-2x^2-2y^2\\ \end{array}\right)
R=Ru(θ)=Rq=⎝⎛1−2y2−2z22xy+2wz2xz−2wy2xy−2wz1−2x2−2z22yz+2wx2xz+2wy2yz−2wx1−2x2−2y2⎠⎞ 四元数和旋转有篇知乎文章讲的很详细。
1.4 坐标系
点
P
P
P在相机坐标系
O
i
j
k
Oijk
Oijk下的坐标表示为
P
=
(
x
,
y
,
z
)
P=(x,y,z)
P=(x,y,z),在世界坐标系
O
w
i
w
j
w
k
w
O_wi_wj_wk_w
Owiwjwkw下的坐标表示为
P
w
=
(
x
w
,
y
w
,
z
w
)
P_w=(x_w,y_w,z_w)
Pw=(xw,yw,zw),相机坐标系到世界坐标系的变化为
R
,
T
R,T
R,T,则有:
P
=
R
P
w
+
T
P=RP_w+T
P=RPw+T
证明:
R , T R,T R,T的涵义:坐标系 O i j k Oijk Oijk先旋转 R R R,再平移 T T T,就变成坐标系 O w i w j w k w O_wi_wj_wk_w Owiwjwkw。即 ( i w , j w , k w ) = ( i , j , k ) R (i_w,j_w,k_w)=(i,j,k)R (iw,jw,kw)=(i,j,k)R,点 O w O_w Ow在 O i j k Oijk Oijk下的坐标为 T T T。 i , j , k i,j,k i,j,k分别为坐标系的一组基,即 X , Y , Z X,Y,Z X,Y,Z轴的单位向量。
P P P在一个坐标系 O i j k Oijk Oijk下的坐标表示为 P = ( x , y , z ) P=(x,y,z) P=(x,y,z)只是简写,完整的表示为: P = ( i , j , k ) ( x y z ) P=(i,j,k)\left(\begin{array}{l}x\\y\\z\end{array}\right) P=(i,j,k)⎝⎛xyz⎠⎞。
所以:
O P → = ( i , j , k ) ( x y z ) \overrightarrow{OP}=(i,j,k)\left(\begin{array}{l}x\\y\\z\end{array}\right) OP=(i,j,k)⎝⎛xyz⎠⎞
O w P → = ( i w , j w , k w ) ( x w y w z w ) = ( i , j , k ) R ( x w y w z w ) \overrightarrow{O_wP}=(i_w,j_w,k_w)\left(\begin{array}{l}x_w\\y_w\\z_w\end{array}\right)=(i,j,k)R\left(\begin{array}{l}x_w\\y_w\\z_w\end{array}\right) OwP=(iw,jw,kw)⎝⎛xwywzw⎠⎞=(i,j,k)R⎝⎛xwywzw⎠⎞
O O w → = ( i , j , k ) T \overrightarrow{OO_w}=(i,j,k)T OOw=(i,j,k)T
由 O P → = O O w → + O w P → \overrightarrow{OP}=\overrightarrow{OO_w}+\overrightarrow{O_wP} OP=OOw+OwP,可得:
( x y z ) = R ( x w y w z w ) + T \left(\begin{array}{l}x\\y\\z\end{array}\right)=R\left(\begin{array}{l}x_w\\y_w\\z_w\end{array}\right)+T ⎝⎛xyz⎠⎞=R⎝⎛xwywzw⎠⎞+T
推论:
1, R R R的列向量分别表示i_w,j_w,k_w在相机坐标系下的坐标
2, R T R^T RT的列向量分别表示i,j,k在世界坐标系下的坐标
3, O w O_w Ow在相机坐标系下的坐标为 T T T
4, O O O在世界坐标系下的坐标为 − R T T -R^TT −RTT
所以,相机在世界坐标系中的位姿表示为: R T R^T RT和 − R T T -R^TT −RTT
点在不同坐标系下的坐标表示之间的变换关系,和将一个点变换到另一个点之间的变换关系,两者容易搞混,要区分清楚。