【3D视觉学习笔记2】摄像机的标定、畸变的建模、2D/3D变换

本系列笔记是北邮鲁老师三维重建课程笔记，视频可在B站找到。

1. 摄像机的标定

摄像机标定的过程就是从1张或者多张图片中求解相机的内外参数的过程。

根据上一节的知识，针孔摄像机模型的世界坐标系到成像平面的映射关系为
$$p=K[R,T]P$$
其中$P$是世界坐标系的坐标，$p$是成像坐标系的坐标。令$M=K[R,T]=[m_1,m_2,m_3{]}^T$，得到
$$p_i=\left[\begin{array}{c}{u}_i\\ v_i\end{array}\right]=\left[\begin{array}{c}\frac{m_1{P}_i}{m_3{P}_i}\\ \frac{m_2{P}_i}{m_3{P}_i}\end{array}\right]$$
通常，我们可以采用这种标准化的格子来进行标定。例如下图的装置，假定三个面的交点就是世界坐标系原点，每个格子的宽度我们是知道的，所以某个点在世界坐标系的坐标值我们可以立即得到。相应地，这个点在成像平面的像素位置我们也知道。如图所示，世界坐标中的点$P_i$就被映射到了$p_i$. 我们可以采集$n$个点。

我们将映射关系重写为
$$\begin{gathered} u_i(m_3{P}_i)=m_1{P}_i\to m_1{P}_i-u_i(m_3{P}_i)=0 \\ {v}_i(m_3{P}_i)=m_2{P}_i\to m_2{P}_i-v_i(m_3{P}_i)=0 \end{gathered}$$
所以，一对$p_i,P_i$可以贡献2个方程。根据第一节的学习，我们知道$M$是有11个自由度（5内参+6外参），所以至少需要6对点才能求解。但是我们采点的过程中，难免有噪声，因此一般是尽可能多采，然后解一个超定方程。

对于线性齐次超定方程$Ax=0$, 最小二乘解的目标函数是$\min ||y-Ax||=\min ||Ax||$. 如果我们限定$||x||=1,$那么可以通过SVD求得。

对A进行SVD得到$A=U\Sigma V^T$.我们要最小化$||Ax||$, 就可以取$A$最小的奇异值对应的奇异向量（也就是$V$的最后一列）。因为$A{v}_i={\sigma }_{v_i}{u}_i$ , 所以$||A{v}_i||={\sigma }_{v_i}$(因为$U$是酉矩阵，$||u_i||=1$)，所以我们就应该取最小的奇异值对应的奇异向量作为解。

我们先写这个方程的形式。我们把$M$拉成列向量：
$$m:=\left[\begin{array}{c}{m}_1^T\\ m_2^T\\ m_3^T\end{array}\right]\in {\mathbb{R}}^{12}$$

别忘了$m_i$是行向量

我们要将上面的方程写成$Pm=0$的形式，现在我们来找这个$P$.

如果我们找了$n$对点，我们可以列出$2n$个方程如下：
$$\begin{gathered} -u_1(m_3{P}_1)+m_1{P}_1=0 \\ -v_1(m_3{P}_1)+m_2{P}_1=0 \\ ... \\ -u_n(m_3{P}_n)+m_1{P}_n=0 \\ -v_n(m_3{P}_n)+m_2{P}_n=0 \end{gathered}$$
对于其中一对点的两个方程，两边取转置，我们可以得到
$$\begin{gathered} P_i^T{m}_1^T+0^T{m}_2^T-u_i{P}_i^T{m}_3^T=0 \\ {0}^T{m}_1^T+P_i^T{m}_2^T-v_i{P}_i^T{m}_3^T=0 \end{gathered}$$
所以，我们令
$$P=\left[\begin{array}{c}{P}_1^T{0}^T-u_1{P}_1^T\\ 0^T{P}_1^T-v_1{P}_1^T\\ ...\\ P_n^T{0}^T-u_n{P}_n^T\\ 0^T{P}_n^T-v_n{P}_n^T\end{array}\right]$$
有$Pm=0$.

所以，求解的步骤就是：我们对$P$进行SVD，然后求出最小奇异值对应的右奇异向量（$V$的最后一列）， 这个向量就是解出的$\hat{m}$. 然后我们将$\hat{m}$重新整理，得到$3\times 4$的矩阵$\hat{M}$.

**但是！**我们通过这种SVD的方式，限定了$||m||=1$. 所以，真正的矩阵$M$是和求解出的$\hat{M}$差一个比例系数$\rho$.

好，为了避免混乱，我们重新整理一下我们现在得到的结果：

1. 世界坐标到像素坐标的映射为$p=MP=K[R,T]P$

2. 内参矩阵为($\theta$是传感器横纵边的夹角，与工艺有关，理想情况$\theta$是直角，推导在后文补充)
   $$\left[\begin{array}{ccc}\alpha & -\alpha \cot \theta & u_0\\ 0& \beta /\sin \theta & v_0\\ 0& 0& 1\end{array}\right]$$

3. 外参矩阵，写成如下形式：
   $$R=\left[\begin{array}{c}{r}_1^T\\ r_2^T\\ r_3^T\end{array}\right],T=\left[\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right]$$
   其中$r_i\in {\mathbb{R}}^3$是$R$的行向量（它是列向量，所以转置是行），$t_i\in \mathbb{R}$.

4. 我们把$M$乘开，得到:
   $$M=\left[\begin{array}{cc}\alpha r_1^T-\alpha \cot \theta r_2^T+u_0{r}_3^T& \alpha t_1-\alpha \cot \theta t_2+u_0{t}_3\\ \frac{\beta }{\sin \theta }{r}_2^T+v_0{r}_3^T& \frac{\beta }{\sin \theta }{t}_2+v_0{t}_3\\ r_3^T& t_3\end{array}\right]=\rho \hat{M}$$

5. 我们现在已知的是$\hat{M}$，可以将其进一步写成$\hat{M}=[A,b]$, 这里面的元素都是已知的。我们未知的是$K,R,T,\rho$.

6. 别忘了旋转矩阵的性质：$r_1,r_2,r_3$相互正交，且模为1.

2.1 $\rho ,u_0,v_0$的获取

我们继续推导：
$$\rho \hat{M}=\rho [A,b]=[...]$$
所以
$$\rho A=\rho \left[\begin{array}{c}{a}_1^T\\ a_2^T\\ a_3^T\end{array}\right]=\left[\begin{array}{c}\alpha r_1^T-\alpha \cot \theta r_2^T+u_0{r}_3^T\\ \frac{\beta }{\sin \theta }{r}_2^T+v_0{r}_3^T\\ r_3^T\end{array}\right]=KR$$
根据最后一行，立即得到
$$\rho a_3^T=r_3^T$$
两边取模：
$$|\rho ||a_3|=1\to \rho =\pm \frac{1}{|a_3|}$$
因此$\rho$就确定了。

然后，我们再考察最后一行点乘中间一行：
$$\begin{gathered} (\rho a_3^T)\cdot (\rho a_2^T)={\rho }^2(a_2\cdot a_3) \\ =\frac{\beta }{\sin \theta }(r_2\cdot r_3)+v_0|r_3| \\ (正交性，模为1)=v_0 \end{gathered}$$
所以$v_0$确定了为$v_0={\rho }^2(a_2\cdot a_3)$

同理，我们考察最后一行点乘第一行，**确定$u_0$**为$u_0={\rho }^2(a_1\cdot a_3)$.

我们用了点乘，也可以用叉乘。

根据正交向量之间叉乘的关系，我们分别让最后一行叉乘第一行和第二行：
$$\begin{gathered} {\rho }^2(a_1\times a_3)=\alpha r_2-\alpha \cot \theta r_1 \\ {\rho }^2(a_2\times a_3)=\frac{\beta }{\sin \theta }{r}_1 \end{gathered}$$
两边取模：
$$\begin{gathered} {\rho }^2|(a_1\times a_3)|=|\alpha r_2-\alpha \cot \theta r_1| \\ {\rho }^2|(a_2\times a_3)|=\frac{|\beta |}{\sin \theta } \end{gathered}$$
而
$$\begin{gathered} |\alpha r_2-\alpha \cot \theta r_1{|}^2=(\cdot {)}^T(\cdot )={\alpha }^2+{\alpha }^2{\cot }^2\theta \\ (1+{\cot }^2\theta =1/{\sin }^2\theta )=(\frac{\alpha }{\sin \theta }{)}^2 \end{gathered}$$
由于$\theta$是一个0~90度之间的值，所以
$$\begin{gathered} {\rho }^2|(a_1\times a_3)|=\frac{|\alpha |}{\sin \theta } \\ {\rho }^2|(a_2\times a_3)|=\frac{|\beta |}{\sin \theta } \end{gathered}$$
考察
$$\begin{gathered} {\rho }^2(a_1\times a_3)\cdot {\rho }^2(a_2\times a_3)=-\alpha \beta \frac{\cos \theta }{{\sin }^2\theta } \\ {\rho }^2|(a_1\times a_3)|\cdot {\rho }^2|(a_2\times a_3)|=\frac{|\alpha ||\beta |}{{\sin }^2\theta } \end{gathered}$$
两式相除（$\alpha ,\beta >0$，可以消去绝对值），得到
$$\cos \theta =-\frac{(a_1\times a_3)\cdot (a_2\times a_3)}{|(a_1\times a_3)|\cdot |(a_1\times a_3)|}$$
这样$\theta$也被确定了, 就可以立即得到
$$\begin{gathered} \alpha ={\rho }^2|(a_1\times a_3)|\sin \theta \\ \beta ={\rho }^2|(a_2\times a_3)|\sin \theta \end{gathered}$$
这样$\alpha ,\beta$也被确定了.

再返回去看之前叉乘的结果，根据${\rho }^2(a_2\times a_3)=\frac{\beta }{\sin \theta }{r}_1$, 由于$r_1$是一个单位向量，指示着向量$a_2\times a_3$的方向，所以我们对$a_2\times a_3$单位化就能得到其方向上的单位向量：
$$r_1=\frac{a_2\times a_3}{|a_2\times a_3|}$$
此外，根据$\rho a_3^T=r_3^T$，代入$\rho$立即得到$r_3=\pm \frac{a_3}{|a_3|}$. 根据正交关系，立即得到$r_2=r_1\times r_3$.

这样$r_2,r_1,r_3$也被确定了. 因此我们得到了$R$. 根据
$$\begin{gathered} \rho \hat{M}=\rho [A,b]=K[R,T] \\ \to \rho b=KT \\ T=\rho K^{-1}b \end{gathered}$$
这样$T$也被确定了，至此我们得到了所有的参数，总结如下：

2. 畸变与畸变下的标定

之前说过桶型和枕型畸变。我们首先对畸变进行建模。

对于桶型畸变，越靠外侧的点越会被拉伸到原点

理想情况下到成像平面的点坐标为$p=MP$, 但需要对$x,y$轴乘缩放因子，因此是
$$p=\left[\begin{array}{ccc}1/\lambda & 0& 0\\ 0& 1/\lambda & 0\\ 0& 0& 1\end{array}\right]MP$$
我们用多项式定义$\lambda =1+{\sum }_{p=1}^3{k}_p{d}^{2p}$

所以$k_p=0$时表示无畸变，称为畸变因子。$d=x^2+y^2$表示到原点的距离。

相反地，对于枕型畸变，越靠外侧的点越会被拉远到原点，所以$\lambda =1-{\sum }_{p=1}^3{k}_p{d}^{2p}$

因此，令
$$Q=\left[\begin{array}{ccc}1/\lambda & 0& 0\\ 0& 1/\lambda & 0\\ 0& 0& 1\end{array}\right]M=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\end{array}\right]M$$
按照上一节的方式，我们也可以写出
$$\begin{gathered} -u_1(q_3{P}_1)+q_1{P}_1=0 \\ -v_1(q_3{P}_1)+q_2{P}_1=0 \\ ... \end{gathered}$$
但是！$q$里面含有未知量（畸变因子），所以上述方程不再是线性方程，需要通过牛顿法或者L-M方法迭代求解。

然而，我们还可以更进一步。根据$q_1=\frac{1}{\lambda }{m}_1,q_2=\frac{1}{\lambda }{m}_2,q_3=m_3$, 则
$$p_i=\left[\begin{array}{c}\frac{q_1{P}_i}{q_3{P}_i}\\ \frac{q_2{P}_i}{q_3{P}_i}\end{array}\right]=\frac{1}{\lambda }\left[\begin{array}{c}\frac{m_1{P}_i}{m_3{P}_i}\\ \frac{m_2{P}_i}{m_3{P}_i}\end{array}\right]$$
我们消去$\lambda$:
$$\frac{u_i}{v_i}=\frac{m_1{P}_i}{m_2{P}_i}$$
得到$u_i{m}_2{P}_i-v_i{m}_1{P}_i=0$, 这又是一个线性方程组，可以按照同样的方法求出$m_1,m_2$，这样就减少了牛顿法或L-M方法所需要迭代的参数量。

3. 2D平面和3D空间的变换

如果两个点A,B经过某种变换后，各自变成A’, B’, 但AB的距离和A’B’的距离一样，称这个变换是等距变换，具有下面的形式：
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}\sigma \cos \theta & -\sin \theta & x_0\\ \sigma \sin \theta & \cos \theta & y_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$
其中，$\sigma =1$时称为保向变换，即单纯的旋转/平移操作，而$\sigma =-1$是镜像变换，如下图：

更广义地，仿射变换如下：
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}{A}_{3\times 3}& t_{3\times 1}\\ 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$
不变量:平行性不变，面积的比值不变，平行线段长度的比值不变。

类似地，在3D空间中，定义仿射变换
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}{A}_{3\times 3}& t_{3\times 1}\\ 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$$
不变量：保持无穷远平面不变（无穷远点变换到无穷远点)、保持直线与直线、直线与平面、平面与平面的平行性不变

如果左下角不再是0，而是一个向量$v$，则称为透视变换：
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}{A}_{3\times 3}& t_{3\times 1}\\ v_{3\times 1}^T& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$$
不变量:点变换到点，线变换到线，保持点的共线(面)性、线的共面性.