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
点在不同坐标系下的坐标表示之间的变换关系,和将一个点变换到另一个点之间的变换关系,两者容易搞混,要区分清楚。