2012年就接触了三维视觉和SLAM，monoSLAM，基于EKF的。当时这个方向还没有起来，现在在ARVR、机器人、自动驾驶中都需SLAM技术。疫情出不去，居家办公之余，决定重温一下三维视觉和SLAM这块的知识。
网上看了北邮鲁鹏老师的课程《计算机视觉之三维重建（深入浅出sfm和SLAM核心算法）》，讲的非常好，适合入门和温顾。三维视觉基础部分讲的很细，最后用分别用OpenMVG和ORB-SLAM框架来讲SFM和SLAM。这边整理一下学这门课的笔记（如果之前能够做笔记，现在也不至于网上到处找资料温习，十年换来的教训啊，以后要及时做笔记）

1，数学基础

注：这边的基础是建立在最基本的线性代数上的，矩阵、向量、行列式、秩、单位矩阵、SVD分解等等基本的概念是本节的前提。

1.1 最小二乘

1.1.1 线性方程组

问题：求解线性方程组Ax=yAx=yAx=y
其中A=(a11…a1q⋮⋱⋮ap1…apq)x=(x1⋮xq)y=(y1⋮yp)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>qp>qp>q

方程无解，要求解的是x∗=argmin⁡x∣∣Ax−y∣∣2x^*=arg\min_x||Ax-y||^2x∗=argminx∣∣Ax−y∣∣2

解法1

x∗=(ATA)−1ATyx^*=(A^TA)^{-1}A^Tyx∗=(ATA)−1ATy

证明：
f(x)=∣∣Ax−y∣∣2=xTATAx−2yTAx+yTyf(x)=||Ax-y||^2=x^TA^TAx-2y^TAx+y^Tyf(x)=∣∣Ax−y∣∣2=xTATAx−2yTAx+yTy
最小值处偏导为0：∂f∂x∣x∗=2ATAx∗−2ATy=0\frac{\partial{f}}{\partial{x}}|_{x^*}=2A^TAx^*-2A^Ty=0∂x∂f∣x∗=2ATAx∗−2ATy=0
⇒x∗=(ATA)−1ATy\Rightarrow x^*=(A^TA)^{-1}A^Ty⇒x∗=(ATA)−1ATy
注：若F(x)=Ax,则∂F∂x=AT，∂F∂x定义为(∂F1∂x1…∂Fp∂x1⋮⋱⋮∂F1∂xq…∂Fp∂xq)若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∗=Vbx^*=Vbx∗=Vb
其中：奇异值分解A=UDVTA=UDV^TA=UDVT，xˉ=VTx,yˉ=UTy,bi=yiˉ/di,di=Dii\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为对角矩阵DDD的对角

证明：
f(x)=∣∣Ax−y∣∣2=∣∣UDVTx−UUTy∣∣2=∣∣U∣∣2∣∣Dxˉ−yˉ∣∣2=∑i=1q(dixˉi−yˉi)2+∑i=q+1pyˉi2f(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^2f(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/di=bi\bar{x}_i=\bar{y}_i/d_i=b_ixˉi=yˉi/di=bi，即xˉ=b\bar{x}=bxˉ=b，所以VTx=bV^Tx=bVTx=b
得到：x=Vbx=Vbx=Vb

解法3

无约束优化问题，用梯度下降、牛顿法、LM求解

1.1.2 齐次线性方程组

Ax=0Ax=0Ax=0
方程无非0解，要求解的是约束线性优化问题：
x∗=argmin⁡x∣∣Ax∣∣2s.t.∣∣x∣∣2=1x^*=arg\min_x||Ax||^2\\s.t.||x||^2=1x∗=argxmin∣∣Ax∣∣2s.t.∣∣x∣∣2=1

解法

x∗=V(0⋮01)x^*=V\left(\begin{array}{l}0\\ \vdots\\0\\1\end{array}\right)x∗=V⎝⎜⎜⎜⎛0⋮01⎠⎟⎟⎟⎞为VVV的最后一列

证明：
f(x)=∣∣Ax∣∣2=∣∣UDVTx∣∣2=∣∣Dxˉ∣∣2=∑i=1q(dixˉi)2=(d12,⋯,dq2)(xˉ12⋮xˉq2)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⋮01)\bar{x}=\left(\begin{array}{l}0\\ \vdots\\0\\1\end{array}\right)xˉ=⎝⎜⎜⎜⎛0⋮01⎠⎟⎟⎟⎞，进而得到x∗x^*x∗

1.1.3 非线性方程组

{f1(x1,⋯,xq)=0f2(x1,⋯,xq)=0⋮fp(x1,⋯,xq)=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∗=argmin⁡x12∑i∣∣fi(x)∣∣2x^*=arg\min_x\frac{1}{2}\sum_i||f_i(x)||^2x∗=argminx21∑i∣∣fi(x)∣∣2，用梯度下降、牛顿法、LM求解

1.2 牛顿法与LM法

问题：求x∗=arg min⁡xf(x)x^*=\argmin_xf(x)x∗=xargminf(x) 对f(x)f(x)f(x)进行泰勒展开：
f(x)=f(x0)+f′(x0)(x−x0)+12f′′(x0)(x−x0)2+⋯f(x)=f(x_0)+f'(x_0)(x-x_0)+\frac{1}{2}f''(x_0)(x-x_0)^2+\cdotsf(x)=f(x0)+f′(x0)(x−x0)+21f′′(x0)(x−x0)2+⋯

梯度下降

xn+1=xn−αf′(xn)x_{n+1}=x_n-\alpha f'(x_n)xn+1=xn−αf′(xn)

证明
用泰勒展开的一阶导数来近似：
f(xn+1)=f(xn)+f′(xn)(xn+1−xn)=f(xn)−α(f′(xn))2f(x_{n+1})=f(x_n)+f'(x_n)(x_{n+1}-x_n)=f(x_n)-\alpha(f'(x_n))^2f(xn+1)=f(xn)+f′(xn)(xn+1−xn)=f(xn)−α(f′(xn))2
当xnx_nxn不是最小值时f′(xn)≠0f'(x_n)\ne0f′(xn)=0，所以f(xn+1)<f(xn)f(x_{n+1})<f(x_n)f(xn+1)<f(xn)，在逼近最优值x∗x^*x∗。

牛顿法

用泰勒展开的二阶导数来近似：f(x)=f(x0)+f′(x0)(x−x0)+12f′′(x0)(x−x0)2f(x)=f(x_0)+f'(x_0)(x-x_0)+\frac{1}{2}f''(x_0)(x-x_0)^2f(x)=f(x0)+f′(x0)(x−x0)+21f′′(x0)(x−x0)2求导得：f′(x)=f′(x0)+f′′(x0)(x−x0)f'(x)=f'(x_0)+f''(x_0)(x-x_0)f′(x)=f′(x0)+f′′(x0)(x−x0)最小值处导数为f′(x)=0f'(x)=0f′(x)=0：x=x0−(f′′(x0))−1f′(x0)x=x_0-(f''(x_0))^{-1}f'(x_0)x=x0−(f′′(x0))−1f′(x0)如果xxx是多维向量，则f′′(x)f''(x)f′′(x)为f(x)f(x)f(x)的Hessian矩阵H(x)=(∂2f∂2x1⋯∂2f∂x1∂xn⋮⋱⋮∂2f∂xn∂x1⋯∂2f∂2xn)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⎠⎟⎟⎞所以，牛顿法的迭代公式为：xn+1=xn−H−1f′(xn)x_{n+1}=x_n-H^{-1} f'(x_n)xn+1=xn−H−1f′(xn)

高斯牛顿法

牛顿法中HHH矩阵要求二阶导数，实际中很难操作，用雅可比矩阵来代替H≈JTJH\approx J^TJH≈JTJ：xn+1=xn−(JTJ)−1f′(xn)x_{n+1}=x_n-(J^TJ)^{-1} f'(x_n)xn+1=xn−(JTJ)−1f′(xn) 这边的J(x)=▽f(x)=f′(x)=(∂f∂x1,⋯,∂f∂xn)TJ(x)=\triangledown f(x)=f'(x)=(\frac{\partial f}{\partial x_1},\cdots,\frac{\partial f}{\partial x_n})^TJ(x)=▽f(x)=f′(x)=(∂x1∂f,⋯,∂xn∂f)T

高斯牛顿这边的近似HHH矩阵不满秩，无法求逆，其实高斯牛顿法不是这样用的，常用在最小二乘问题中，把这边的fff变成多个方程的fif_ifi，雅可比矩阵JJJ就能是满秩了。

LM

（Levenberg-Marquadt）加一个缩放的单位矩阵μI\mu IμI，防止JTJJ^TJJTJ不满秩：
xn+1=xn−(JTJ+μI)−1f′(xn)x_{n+1}=x_n-(J^TJ+\mu I)^{-1} f'(x_n)xn+1=xn−(JTJ+μI)−1f′(xn) 相当于梯度下降和高斯牛顿的混合。在最小二乘问题中，同高斯牛顿法，这边的JJJ可以换成向量函数(fi)(f_i)(fi)对xxx的雅可比矩阵。

1.3 变换

1.3.1 2D变换

x=(xy1)，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′=(Rt01)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′=(sRt01)x\boldsymbol{x'}=\left(\begin{array}{c}sR&t\\0&1\end{array}\right)\boldsymbol{x}x′=(sR0t1)x
仿射变换
x′=(At01)x\boldsymbol{x'}=\left(\begin{array}{c}A&t\\0&1\end{array}\right)\boldsymbol{x}x′=(A0t1)x
透视变换
x′=(AtvT1)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,ZX,Y,ZX,Y,Z轴的旋转α,β,γ\alpha,\beta,\gammaα,β,γ角度，可以得到欧式变换：
R=Rx(α)Ry(β)Rz(γ)R=R_x(\alpha)R_y(\beta)R_z(\gamma)R=Rx(α)Ry(β)Rz(γ) 其中Rx(α)=(1000cos⁡α−sin⁡α0sin⁡α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α⎠⎞ Ry(β)=(cos⁡β0sin⁡β010−sin⁡β0cos⁡β)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β⎠⎞ Rz(γ)=(cos⁡γ−sin⁡γ0sin⁡γcos⁡γ0001)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=(ux,uy,uz)u=(u_x,u_y,u_z)u=(ux,uy,uz)旋转θ\thetaθ角度，表示为Ru(θ)R_u(\theta)Ru(θ)
用四元数表示：q=(x,y,z,w)=(uxsin⁡(θ/2),uysin⁡(θ/2),uzsin⁡(θ/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=Ru(θ)=Rq=(1−2y2−2z22xy−2wz2xz+2wy2xy+2wz1−2x2−2z22yz−2wx2xz−2wy2yz+2wx1−2x2−2y2)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 坐标系

点PPP在相机坐标系OijkOijkOijk下的坐标表示为P=(x,y,z)P=(x,y,z)P=(x,y,z)，在世界坐标系OwiwjwkwO_wi_wj_wk_wOwiwjwkw下的坐标表示为Pw=(xw,yw,zw)P_w=(x_w,y_w,z_w)Pw=(xw,yw,zw)，相机坐标系到世界坐标系的变化为R,TR,TR,T，则有：
P=RPw+TP=RP_w+TP=RPw+T

证明：
R,TR,TR,T的涵义：坐标系OijkOijkOijk先旋转RRR，再平移TTT，就变成坐标系OwiwjwkwO_wi_wj_wk_wOwiwjwkw。即(iw,jw,kw)=(i,j,k)R(i_w,j_w,k_w)=(i,j,k)R(iw,jw,kw)=(i,j,k)R，点OwO_wOw在OijkOijkOijk下的坐标为TTT。i,j,ki,j,ki,j,k分别为坐标系的一组基，即X,Y,ZX,Y,ZX,Y,Z轴的单位向量。
PPP在一个坐标系OijkOijkOijk下的坐标表示为P=(x,y,z)P=(x,y,z)P=(x,y,z)只是简写，完整的表示为：P=(i,j,k)(xyz)P=(i,j,k)\left(\begin{array}{l}x\\y\\z\end{array}\right)P=(i,j,k)⎝⎛xyz⎠⎞。
所以：
OP→=(i,j,k)(xyz)\overrightarrow{OP}=(i,j,k)\left(\begin{array}{l}x\\y\\z\end{array}\right)OP=(i,j,k)⎝⎛xyz⎠⎞
OwP→=(iw,jw,kw)(xwywzw)=(i,j,k)R(xwywzw)\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⎠⎞
OOw→=(i,j,k)T\overrightarrow{OO_w}=(i,j,k)TOOw=(i,j,k)T
由OP→=OOw→+OwP→\overrightarrow{OP}=\overrightarrow{OO_w}+\overrightarrow{O_wP}OP=OOw+OwP，可得：
(xyz)=R(xwywzw)+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，RRR的列向量分别表示i_w,j_w,k_w在相机坐标系下的坐标
2，RTR^TRT的列向量分别表示i,j,k在世界坐标系下的坐标
3，OwO_wOw在相机坐标系下的坐标为TTT
4，OOO在世界坐标系下的坐标为−RTT-R^TT−RTT
所以，相机在世界坐标系中的位姿表示为：RTR^TRT和−RTT-R^TT−RTT

点在不同坐标系下的坐标表示之间的变换关系，和将一个点变换到另一个点之间的变换关系，两者容易搞混，要区分清楚。

《计算机视觉之三维重建》笔记1-数学基础相关推荐

《计算机视觉之三维重建》笔记3-单视几何
北邮鲁鹏老师的课程<计算机视觉之三维重建(深入浅出sfm和SLAM核心算法)> 笔记 3,单视几何 3.1 2D 直线平面上直线用 l = ( a , b , c ) T l=(a,b, ...
2021/06/29计算机视觉期末复习笔记整理
计算机视觉期末复习笔记整理引言我的复习参考期末考试考题回忆 PPT对应中文笔记整理参考的几篇博客的笔记引言刚结束可能是我学生时代最后一场考试了,orz热乎着,记录一下. 这门课是学校新开的 ...
检测单击鼠标左键并拖动的消息_计算机视觉OpenCV学习笔记（四）：关于鼠标的相关事件函数...
(7)把鼠标当画笔本篇目标: 学会使用OpenCV中的鼠标处理的相关事件,事件回调函数怎么去定义,参数有哪些,以及如何注册鼠标监听事件. 7.1 .1 回调函数的定义: 1 def name(eve ...
9月16日计算机视觉基础学习笔记——认识机器视觉
文章目录前言一.Week 1 homework 1.基本操作 2.滤波二.从图像处理到计算机视觉 1.反色变换 2.Gamma 变换 3.直方图 & 直方图均衡化三.计算机视觉的两个步 ...
CQF笔记Primer数学基础
CQF笔记Primer数学基础 1 Calculus 微积分 1.1 Basic Terminology 基本术语 1.2 Functions 函数 1.2.1 Explicit/Implicit R ...
10月15日计算机视觉基础学习笔记——分割网络的设计
文章目录前言一.分割器的设计 1.优化:减小 feature map 2.上采样 up sampling 二.经典分割模型的涨点方法前言本文为10月15日计算机视觉基础学习笔记--分割网络的设 ...
9月14日计算机视觉基础学习笔记——基本图像处理
文章目录前言一.计算机视觉的由来二.计算机如何看到图像三.计算机处理图像的方式.方法 1.直接从 camera 读取 2.从文件读取 3.生成矩阵显示四.图像处理 1.颜色空间转换 (1). ...
年终总结 | 2019年人工智能+计算机视觉+深度学习笔记思维导图汇总
点上方蓝字计算机视觉联盟获取更多干货在右上方 ··· 设为星标 ★,与你不见不散本文主要回顾计算机视觉.人工智能.互联网公司.自动驾驶.手推笔记.Python操作等核心内容篇幅有限,只能总结这些 ...
精讲CS231N斯坦福计算机视觉公开课笔记
[子豪兄]精讲CS231N斯坦福计算机视觉公开课计算机视觉发展历史与课程概述大概讲了一些计算机视觉的运用范围(机器学习.生物.无人机.光学等).Hubel和Wiesel在1959年发现猫对图像的边 ...

《计算机视觉之三维重建》笔记1-数学基础