相机标定（张正友标定算法）解读与实战一

参考论文：

https://www.microsoft.com/en-us/research/wp-content/uploads/2016/02/tr98-71.pdf

针孔相机模型

世界坐标系
相机坐标系
归一化平面坐标系
像平面坐标系
像素坐标系

关于相机模型的文章太多了，要搞清楚，首先要弄清楚这5个坐标系，以及他们之间的转换关系。我这里开始由世界坐标系一步一步推导到像素坐标系进行讲解。

• 世界坐标系到相机坐标系

  实际上是两个三维坐标系之间的转换关系，属于刚体变换，也就是说只有旋转R和平移t变换。旋转矩阵R是正交矩阵的单位矩阵，并且R的转置等于R的逆。$X_c$相机坐标系的点，$X_w$世界坐标系的点。
  .
  $$X_c=\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right],X_w=\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]$$

$$\begin{gathered} X_c=R{X}_w+t \\ {R}^T=R^{-1} \end{gathered}$$
R表示旋转矩阵，t表示平移向量，如果把$X_c,X_w$写成齐次向量的形式，那么他们之间的变换关系就可以写成矩阵相乘的形式。

$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$

相机坐标系和世界坐标系之间可以互相转换，其中最常用的就是如何确定相机中心在世界坐标系中的位置
$$\begin{gathered} X_w=R^{-1}(X_c-t) \\ =R^{-1}{X}_c-R^{-1}t \\ =R^T{X}_c-R^{T}t \end{gathered}$$
由于相机在相机坐标系的坐标为原点 $Xc=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]$
所以相机在世界坐标系中的坐标为：
$$X_w^c=-R^{T}t$$

相机坐标系到像平面坐标系

根据三角形相似
$$\begin{gathered} x=f\frac{x_c}{z_c} \\ y=f\frac{y_c}{z_c} \end{gathered}$$

写成矩阵的形式
$$\begin{gathered} \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}f& 0& 0\\ 0& f& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c/z_c\\ y_c/z_c\\ 1\end{array}\right] \\ \frac{1}{z_c}\left[\begin{array}{ccc}f& 0& 0\\ 0& f& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right] \end{gathered}$$

这里$f$表示焦距，单位是m，${\left[\begin{array}{ccc}{x}_c/z_c& y_c/z_c& 1\end{array}\right]}^T$为归一化平面坐标
像平面坐标系到图像坐标系
由于图像坐标系是用像素表示的，像平面坐标系都是m为单位的，所以要进行转换，
焦距f转换为像素，$d_x,d_y$ 分别表示x和y方向每个像素占多大。$fx,fy$ 的单位是像素
$$\begin{gathered} f_x=\frac{f}{d_x} \\ {f}_y=\frac{f}{d_y} \end{gathered}$$

由于像平面的原点在中心，而像素坐标的原点在左上角，因而需要在x和y方向分别平移 $c_x,c_y$个像素

$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& c_x\\ 0& \frac{1}{d_y}& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

相机坐标系->像平面坐标->像素坐标
$$\begin{gathered} \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& c_x\\ 0& \frac{1}{d_y}& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right] \\ =\frac{1}{z_c}\left[\begin{array}{ccc}\frac{1}{d_x}& 0& c_x\\ 0& \frac{1}{d_y}& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}f& 0& 0\\ 0& f& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right] \\ =\frac{1}{z_c}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right] \end{gathered}$$
其中矩阵为相机内参矩阵，只和相机本身有关 $f_x,f_y$为相机焦距，一般情况向两者相等， $c_x,c_y$为中心偏移像素，一般情况下为图像的1/2。
$$K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$

那么世界坐标系到像素坐标系就可以写成：
$$\begin{gathered} \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right] \\ =\frac{1}{z_c}K[R|t]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right] \end{gathered}$$
其中$[R|t]为外参矩阵，表示世界坐标系到相机坐标系的转换矩阵，K为相机内参矩阵$

$$P=K[R|t]$$
P为投影矩阵，表示一个4x4的矩阵。
关于针孔相机模型就解释到这里。

相机畸变模型

相机畸变模型一般包括径向畸变和切向畸变

• 径向畸变是由于透镜的形状导致的，有桶形畸变和枕形畸变。对应的数学模型:

$$\begin{gathered} x_{dist}=x+x(k_1{r}^2+k_2{r}^4+k_3{r}^6) \\ {y}_{dist}=y+y(k_1{r}^2+k_2{r}^4+k_3{r}^6) \\ r=x^2+y^2 \end{gathered}$$

• 切向畸变是由于安装导致透镜和成像平面不严格平行。对应的数学模型：

$$\begin{gathered} x_{dist}=x+2p_{1}xy+p_2(r^2+2x^2) \\ {y}_{dist}=y+2p_{2}xy+p_1(r^2+2y^2) \end{gathered}$$

同时进行径向畸变和切向畸变矫正，联合在一起：
$$\begin{gathered} x_{dist}=x+x(k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{1}xy+p_2(r^2+2x^2) \\ {y}_{dist}=y+y(k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{2}xy+p_1(r^2+2y^2) \end{gathered}$$

x，y 的值对应的是归一化平面坐标系的值，因而在校正图像的时候需要先把像素坐标转换为归一化平面坐标，然后再校正，最后再把校正后的$x_{dist},y_{dist}$计算到像素坐标。
$$\begin{gathered} u=f_x{x}_{dist}+c_x \\ v=f_y{y}_{dist}+c_y \end{gathered}$$

畸变系数涉及$k_1,k_2,k_3,p_1,p_2$