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

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

1，数学基础

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

1.1 最小二乘

1.1.1 线性方程组

问题：求解线性方程组$Ax=y$
其中$A=\left(\begin{array}{ccc}{a}_{11}& \dots & a_{1q}\\ ⋮& \ddots & ⋮\\ a_{p1}& \dots & a_{pq}\end{array}\right)x=\left(\begin{array}{l}{x}_1\\ ⋮\\ x_q\end{array}\right)y=\left(\begin{array}{l}{y}_1\\ ⋮\\ y_p\end{array}\right)$$A$列满秩且$p>q$

方程无解，要求解的是$x^{\ast }=arg{\min }_x||Ax-y|{|}^2$

解法1

$x^{\ast }=(A^{T}A{)}^{-1}{A}^{T}y$

证明：
$f(x)=||Ax-y|{|}^2=x^T{A}^{T}Ax-2y^{T}Ax+y^{T}y$
最小值处偏导为0：$\frac{\partial f}{\partial x}{|}_{x^{\ast }}=2A^{T}A{x}^{\ast }-2A^{T}y=0$
$\Rightarrow x^{\ast }=(A^{T}A{)}^{-1}{A}^{T}y$
注：$若F(x)=Ax,则\frac{\partial F}{\partial x}=A^T，\frac{\partial F}{\partial x}定义为\left(\begin{array}{ccc}\frac{\partial F_1}{\partial x_1}& \dots & \frac{\partial F_p}{\partial x_1}\\ ⋮& \ddots & ⋮\\ \frac{\partial F_1}{\partial x_q}& \dots & \frac{\partial F_p}{\partial x_q}\end{array}\right)$

解法2

$x^{\ast }=Vb$
其中：奇异值分解$A=UD{V}^T$，$\bar{x}=V^{T}x,\bar{y}=U^{T}y,b_i=\overline{y_i}/d_i,d_i=D_{ii}$为对角矩阵$D$的对角

证明：
$$\begin{gathered} f(x)=||Ax-y|{|}^2 \\ =||UD{V}^{T}x-U{U}^{T}y|{|}^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 \end{gathered}$$
求最小值，则：${\bar{x}}_i={\bar{y}}_i/d_i=b_i$，即$\bar{x}=b$，所以$V^{T}x=b$
得到：$x=Vb$

解法3

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

1.1.2 齐次线性方程组

$Ax=0$
方程无非0解，要求解的是约束线性优化问题：
$$\begin{gathered} x^{\ast }=arg\underset{x}{\min }||Ax|{|}^2 \\ s.t.||x|{|}^2=1 \end{gathered}$$

解法

$x^{\ast }=V\left(\begin{array}{l}0\\ ⋮\\ 0\\ 1\end{array}\right)$为$V$的最后一列

证明：
$$\begin{gathered} f(x)=||Ax|{|}^2 \\ =||UD{V}^{T}x|{|}^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\\ ⋮\\ {\bar{x}}_q^2\end{array}\right) \end{gathered}$$
因为$||x|{|}^2=1$，所以$||\bar{x}|{|}^2=1$
所以，要使$f(x)$最小，则$\bar{x}=\left(\begin{array}{l}0\\ ⋮\\ 0\\ 1\end{array}\right)$，进而得到$x^{\ast }$

1.1.3 非线性方程组

$$\{\begin{array}{c}{f}_1(x_1,\cdots ,x_q)=0\\ f_2(x_1,\cdots ,x_q)=0\\ ⋮\\ f_p(x_1,\cdots ,x_q)=0\end{array}$$
转化为优化问题：$x^{\ast }=arg{\min }_x\frac{1}{2}{\sum }_i||f_i(x)|{|}^2$，用梯度下降、牛顿法、LM求解

1.2 牛顿法与LM法

问题：求$$x^{\ast }=\underset{x}{\mathrm{argmin}}f(x)$$ 对$f(x)$进行泰勒展开：
$$f(x)=f(x_0)+f^{\prime }(x_0)(x-x_0)+\frac{1}{2}{f}^{\prime \prime }(x_0)(x-x_0{)}^2+\cdots$$

梯度下降

$$x_{n+1}=x_n-\alpha f^{\prime }(x_n)$$

证明
用泰勒展开的一阶导数来近似：
$f(x_{n+1})=f(x_n)+f^{\prime }(x_n)(x_{n+1}-x_n)=f(x_n)-\alpha (f^{\prime }(x_n){)}^2$
当$x_n$不是最小值时$f^{\prime }(x_n)\ne 0$，所以$f(x_{n+1})<f(x_n)$，在逼近最优值$x^{\ast }$。

牛顿法

用泰勒展开的二阶导数来近似：$$f(x)=f(x_0)+f^{\prime }(x_0)(x-x_0)+\frac{1}{2}{f}^{\prime \prime }(x_0)(x-x_0{)}^2$$求导得：$$f^{\prime }(x)=f^{\prime }(x_0)+f^{\prime \prime }(x_0)(x-x_0)$$最小值处导数为$f^{\prime }(x)=0$：$$x=x_0-(f^{\prime \prime }(x_0){)}^{-1}{f}^{\prime }(x_0)$$如果$x$是多维向量，则$f^{\prime \prime }(x)$为$f(x)$的Hessian矩阵$$H(x)=\left(\begin{array}{ccc}\frac{{\partial }^{2}f}{{\partial }^2{x}_1}& \cdots & \frac{{\partial }^{2}f}{\partial x_1\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{{\partial }^{2}f}{\partial x_n\partial x_1}& \cdots & \frac{{\partial }^{2}f}{{\partial }^2{x}_n}\end{array}\right)$$所以，牛顿法的迭代公式为：$$x_{n+1}=x_n-H^{-1}{f}^{\prime }(x_n)$$

高斯牛顿法

牛顿法中$H$矩阵要求二阶导数，实际中很难操作，用雅可比矩阵来代替$H\approx J^{T}J$：$$x_{n+1}=x_n-(J^{T}J{)}^{-1}{f}^{\prime }(x_n)$$ 这边的$$J(x)=▽f(x)=f^{\prime }(x)=(\frac{\partial f}{\partial x_1},\cdots ,\frac{\partial f}{\partial x_n}{)}^T$$

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

LM

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

1.3 变换

1.3.1 2D变换

$$\mathbf{x}=\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)，{\mathbf{x}}^{\prime }=\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right)$$

• 欧式变换
  $${\mathbf{x}}^{\prime }=\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right)\mathbf{x}$$ $$R=\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)$$
• 相似变换
  $${\mathbf{x}}^{\prime }=\left(\begin{array}{cc}sR& t\\ 0& 1\end{array}\right)\mathbf{x}$$
• 仿射变换
  $${\mathbf{x}}^{\prime }=\left(\begin{array}{cc}A& t\\ 0& 1\end{array}\right)\mathbf{x}$$
• 透视变换
  $${\mathbf{x}}^{\prime }=\left(\begin{array}{cc}A& t\\ v^T& 1\end{array}\right)\mathbf{x}$$

1.3.2 3D变换

变换公式形同2D变换，维度由2维拓展到3维即可。

1.3.3 3D欧式变换

三维空间的欧式变换（正交变换），有两种几何意义（表示）：

欧拉表示法

分别围绕$X,Y,Z$轴的旋转$\alpha ,\beta ,\gamma$角度，可以得到欧式变换：
$$R=R_x(\alpha )R_y(\beta )R_z(\gamma )$$ 其中$$R_x(\alpha )=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & -\sin \alpha \\ 0& \sin \alpha & \cos \alpha \end{array}\right)$$ $$R_y(\beta )=\left(\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right)$$ $$R_z(\gamma )=\left(\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right)$$

四元数

可以围绕单位向量$u=(u_x,u_y,u_z)$旋转$\theta$角度，表示为$R_u(\theta )$
用四元数表示：$$q=(x,y,z,w)=(u_x\sin (\theta /2),u_y\sin (\theta /2),u_z\sin (\theta /2),\cos (\theta /2))$$ 则欧式变换可表示为：
$$R=R_u(\theta )=R_q=\left(\begin{array}{ccc}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)$$ 四元数和旋转有篇知乎文章讲的很详细。

1.4 坐标系

点$P$在相机坐标系$Oijk$下的坐标表示为$P=(x,y,z)$，在世界坐标系$O_w{i}_w{j}_w{k}_w$下的坐标表示为$P_w=(x_w,y_w,z_w)$，相机坐标系到世界坐标系的变化为$R,T$，则有：
$$P=R{P}_w+T$$

证明：
$R,T$的涵义：坐标系$Oijk$先旋转$R$，再平移$T$，就变成坐标系$O_w{i}_w{j}_w{k}_w$。即$(i_w,j_w,k_w)=(i,j,k)R$，点$O_w$在$Oijk$下的坐标为$T$。$i,j,k$分别为坐标系的一组基，即$X,Y,Z$轴的单位向量。
$P$在一个坐标系$Oijk$下的坐标表示为$P=(x,y,z)$只是简写，完整的表示为：$P=(i,j,k)\left(\begin{array}{l}x\\ y\\ z\end{array}\right)$。
所以：
$\overrightarrow{OP}=(i,j,k)\left(\begin{array}{l}x\\ y\\ z\end{array}\right)$
$\overrightarrow{O_{w}P}=(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)$
$\overrightarrow{O{O}_w}=(i,j,k)T$
由$\overrightarrow{OP}=\overrightarrow{O{O}_w}+\overrightarrow{O_{w}P}$，可得：
$\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$

推论：
1，$R$的列向量分别表示i_w,j_w,k_w在相机坐标系下的坐标
2，$R^T$的列向量分别表示i,j,k在世界坐标系下的坐标
3，$O_w$在相机坐标系下的坐标为$T$
4，$O$在世界坐标系下的坐标为$-R^{T}T$
所以，相机在世界坐标系中的位姿表示为：$R^T$和$-R^{T}T$

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