相机标定

一、相机标定的目的1. 相机的成像过程2. 相机标定的目的3. 畸变与畸变矫正二、张正友标定法简介三、标定相机内外参1. 求解内参和外参的积2. 求解内参矩阵3. 求解外参矩阵四、标定相机的畸变参数五、L-M算法参数优化六、相机标定的步骤七、源代码

---

一、相机标定的目的

我们首先要明白两个问题：1、相机是如何成像的？2、相机标定的目的是什么？

1. 相机的成像过程

相机成像系统中，共包含四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。

这四个坐标系之间的转化关系为：

其中， (U,V,W)(U, V, W)(U,V,W)为在世界坐标系下一点的物理坐标， (u,v)(u, v)(u,v)为该点对应的在像素坐标系下的像素坐标， ZZZ为尺度因子。

我们将矩阵：

(1dX−cotθdXu001dYsinθv0001)(f0000f000010)=(fdX−fcotθdXu000fdYsinθv000010)\left( \begin{matrix} \frac{1}{dX} & -\frac{cot\theta}{dX} & u_0\\ \ \\ 0 & \frac{1}{dYsin\theta} & v_0\\ \ \\ 0 & 0 & 1 \end{matrix} \right) \left( \begin{matrix} f & 0 & 0 & 0\\ \ \\ 0 & f & 0 & 0\\ \ \\ 0 & 0 & 1 & 0 \end{matrix} \right) = \left( \begin{matrix} \frac{f}{dX} & -\frac{fcot\theta}{dX} & u_0 & 0\\ \ \\ 0 & \frac{f}{dYsin\theta} & v_0 & 0\\ \ \\ 0 & 0 & 1 & 0 \end{matrix} \right) ⎝⎜⎜⎜⎜⎛​dX1​00​−dXcotθ​dYsinθ1​0​u0​v0​1​⎠⎟⎟⎟⎟⎞​⎝⎜⎜⎜⎜⎛​f00​0f0​001​000​⎠⎟⎟⎟⎟⎞​=⎝⎜⎜⎜⎜⎛​dXf​00​−dXfcotθ​dYsinθf​0​u0​v0​1​000​⎠⎟⎟⎟⎟⎞​

称为相机的内参矩阵，内参矩阵取决于相机的内部参数。其中，fff为焦距，dX,dYdX, dYdX,dY分别表示X,YX, YX,Y方向上的一个像素在相机感光板上的物理长度（即一个像素在感光板上是多少毫米），u0,v0u_0, v_0u0​,v0​分别表示相机感光板中心在像素坐标系下的坐标，θ\thetaθ表示感光板的横边和纵边之间的角度（90°表示无误差）。

我们将矩阵(RT01)\left(\begin{matrix} R & T\\ 0 & 1\end{matrix}\right)(R0​T1​)称为相机的外参矩阵，外参矩阵取决于相机坐标系和世界坐标系的相对位置，RRR表示旋转矩阵，TTT表示平移矢量。

即单点无畸变的相机成像模型如下：

Z(uv1)=(fdX−fcotθdXu000fdYsinθv000010)(RT01)(UVW1)Z\left(\begin{matrix} u\\ \ \\ v\\ \ \\ 1 \end{matrix}\right) = \left(\begin{matrix} \frac{f}{dX} & -\frac{fcot\theta}{dX} & u_0 & 0\\ \ \\ 0 & \frac{f}{dYsin\theta} & v_0 & 0\\ \ \\ 0 & 0 & 1 & 0 \end{matrix}\right) \left(\begin{matrix} R & T\\ \ \\ 0 & 1\end{matrix}\right) \left(\begin{matrix} U\\ \ \\ V\\ \ \\ W\\ \ \\ 1 \end{matrix}\right) Z⎝⎜⎜⎜⎜⎛​uv1​⎠⎟⎟⎟⎟⎞​=⎝⎜⎜⎜⎜⎛​dXf​00​−dXfcotθ​dYsinθf​0​u0​v0​1​000​⎠⎟⎟⎟⎟⎞​⎝⎛​R0​T1​⎠⎞​⎝⎜⎜⎜⎜⎜⎜⎜⎜⎛​UVW1​⎠⎟⎟⎟⎟⎟⎟⎟⎟⎞​

2. 相机标定的目的

为什么要进行相机标定呢？比如，当我们拿到一张图片，进行识别之后，得到的两部分之间的距离为多少像素，但是这多少像素究竟对应实际世界中的多少米呢？这就需要利用相机标定的结果来将像素坐标转换到物理坐标来计算距离（当然这里值得说明，仅仅利用单目相机标定的结果，是无法直接从像素坐标转化到物理坐标的，因为透视投影丢失了一个维度的坐标，所以测距其实需要双目相机）。

相机标定的目的其实很简单，我们要想对一个成像系统建模，进而进行相应的计算，所必须的参数就是相机的内参矩阵(fdX−fcotθdXu000fdYsinθv000010)\left(\begin{matrix} \frac{f}{dX} & -\frac{fcot\theta}{dX} & u_0 & 0\\ \ \\ 0 & \frac{f}{dYsin\theta} & v_0 & 0\\ \ \\ 0 & 0 & 1 & 0 \end{matrix}\right)⎝⎜⎜⎜⎜⎛​dXf​00​−dXfcotθ​dYsinθf​0​u0​v0​1​000​⎠⎟⎟⎟⎟⎞​和相机的外参矩阵(RT01)\left(\begin{matrix} R & T\\ 0 & 1\end{matrix}\right)(R0​T1​)。因此，相机标定的第一个目的就是获得相机的内参矩阵和外参矩阵。

3. 畸变与畸变矫正

另外，相机拍摄的图片还存在一定的畸变，畸变包括桶形畸变和枕形畸变。

畸变模型包括径向畸变和切向畸变。

径向畸变公式（3阶）如下：

x^=x(1+k1r2+k2r4+k3r6)y^=y(1+k1r2+k2r4+k3r6)\hat{x} = x(1 + k_1r^2 + k_2r^4 + k_3r^6) \\ \ \\ \hat{y} = y(1 + k_1r^2 + k_2r^4 + k_3r^6) x^=x(1+k1​r2+k2​r4+k3​r6)y^​=y(1+k1​r2+k2​r4+k3​r6)

切向畸变公式如下：

x^=x+(2p1y+p2(r2+2x2))y^=y+(p1(r2+2y2)+2p2x)\hat{x} = x + (2p_1y + p_2(r^2 + 2x^2)) \\ \ \\ \hat{y} = y + (p_1(r^2 +2y^2) + 2p_2x) x^=x+(2p1​y+p2​(r2+2x2))y^​=y+(p1​(r2+2y2)+2p2​x)

其中，(x,y),(x^,y^)(x, y), (\hat{x}, \hat{y})(x,y),(x^,y^​)分别为理想的无畸变的归一化的图像坐标、畸变后的归一化图像坐标，rrr为图像像素点到图像中心点的距离，即r2=x2+y2r^2 = x^2 + y^2r2=x2+y2。

相机标定的第二个目的就是获得相机的畸变参数，如上式中的k1,k2,k3,p1,p2k_1, k_2, k_3, p_1, p_2k1​,k2​,k3​,p1​,p2​等，进而对拍摄的图片进行去畸变处理。

二、张正友标定法简介

张正友标定法利用如下图所示的棋盘格标定板，在得到一张标定板的图像之后，可以利用相应的图像检测算法得到每一个角点的像素坐标(u,v)(u, v)(u,v)。

张正友标定法将世界坐标系固定于棋盘格上，则棋盘格上任一点的物理坐标W=0W=0W=0，由于标定板的世界坐标系是人为事先定义好的，标定板上每一个格子的大小是已知的，我们可以计算得到每一个角点在世界坐标系下的物理坐标(U,V,W=0)(U, V, W = 0)(U,V,W=0)。

我们将利用这些信息：每一个角点的像素坐标(u,v)(u, v)(u,v) 、每一个角点在世界坐标系下的物理坐标(U,V,W=0)(U, V, W = 0)(U,V,W=0)，来进行相机的标定，获得相机的内外参矩阵、畸变参数。

三、标定相机内外参

张正友标定法标定相机的内外参数的思路如下：

1）、求解内参矩阵与外参矩阵的积；

2）、求解内参矩阵；

3）、求解外参矩阵。

1. 求解内参和外参的积

将世界坐标系固定于棋盘格上，则棋盘格上任一点的物理坐标W=0W = 0W=0。因此，原单点无畸变的成像模型可以化为下式：

Z(uv1)=(fdX−fcotθdXu00fdYsinθv0001)(R1R2T)(UV1)=A(R1R2T)(UV1)Z\left(\begin{matrix} u\\ \ \\ v\\ \ \\ 1 \end{matrix}\right) = \left(\begin{matrix} \frac{f}{dX} & -\frac{fcot\theta}{dX} & u_0\\ \ \\ 0 & \frac{f}{dYsin\theta} & v_0\\ \ \\ 0 & 0 & 1 \end{matrix}\right) \left(\begin{matrix} R1 & R2 & T\end{matrix}\right) \left(\begin{matrix} U\\ \ \\ V\\ \ \\ 1 \end{matrix}\right) = A\left(\begin{matrix} R1 & R2 & T\end{matrix}\right) \left(\begin{matrix} U\\ \ \\ V\\ \ \\ 1 \end{matrix}\right) Z⎝⎜⎜⎜⎜⎛​uv1​⎠⎟⎟⎟⎟⎞​=⎝⎜⎜⎜⎜⎛​dXf​00​−dXfcotθ​dYsinθf​0​u0​v0​1​⎠⎟⎟⎟⎟⎞​(R1​R2​T​)⎝⎜⎜⎜⎜⎛​UV1​⎠⎟⎟⎟⎟⎞​=A(R1​R2​T​)⎝⎜⎜⎜⎜⎛​UV1​⎠⎟⎟⎟⎟⎞​

其中，R1,R2R1, R2R1,R2为旋转矩阵RRR的前两列。为了简便，将内参矩阵记为AAA。

对于不同的图片，内参矩阵AAA为定值；对于同一张图片，内参矩阵AAA，外参矩阵(R1R2T)\left(\begin{matrix} R1 & R2 & T\end{matrix}\right)(R1​R2​T​)为定值；对于同一张图片上的单点，内参矩阵AAA，外参矩阵(R1R2T)\left(\begin{matrix} R1 & R2 & T\end{matrix}\right)(R1​R2​T​)，尺度因子ZZZ为定值。

我们将A(R1R2T)A\left(\begin{matrix} R1 & R2 & T\end{matrix}\right)A(R1​R2​T​)记为矩阵HHH，HHH即为内参矩阵和外参矩阵的积，记矩阵HHH的三列为(H1H2H3)\left(\begin{matrix} H1 & H2 & H3\end{matrix}\right)(H1​H2​H3​)，则有：

(uv1)=1ZH(UV1)=1Z(H11H12H13H12H22H32H31H32H33)(UV1)\left(\begin{matrix} u\\ v\\ 1 \end{matrix}\right) = \frac{1}{Z}H \left(\begin{matrix} U\\ V\\ 1 \end{matrix}\right) = \frac{1}{Z} \left(\begin{matrix} H_{11} & H_{12} & H_{13}\\ H_{12} & H_{22} & H_{32}\\ H_{31} & H_{32} & H_{33} \end{matrix}\right) \left(\begin{matrix} U\\ V\\ 1 \end{matrix}\right) ⎝⎛​uv1​⎠⎞​=Z1​H⎝⎛​UV1​⎠⎞​=Z1​⎝⎛​H11​H12​H31​​H12​H22​H32​​H13​H32​H33​​⎠⎞​⎝⎛​UV1​⎠⎞​

利用上式，消去尺度因子ZZZ，可得：

u=H11U+H12V+H13H31U+H32V+H33v=H21U+H22V+H23H31U+H32V+H33u = \frac{H_{11}U+H_{12}V+H_{13}}{H_{31}U+H_{32}V+H_{33}}\\ \ \\ v = \frac{H_{21}U+H_{22}V+H_{23}}{H_{31}U+H_{32}V+H_{33}} u=H31​U+H32​V+H33​H11​U+H12​V+H13​​v=H31​U+H32​V+H33​H21​U+H22​V+H23​​

此时，尺度因子ZZZ已经被消去，因此上式对于同一张图片上所有的角点均成立。(u,v)(u, v)(u,v)是像素坐标系下的标定板角点的坐标，(U,V)(U, V)(U,V)是世界坐标系下的标定板角点的坐标。通过图像识别算法，我们可以得到标定板角点的像素坐标(u,v)(u, v)(u,v)，又由于标定板的世界坐标系是人为定义好的，标定板上每一个格子的大小是已知的，我们可以计算得到世界坐标系下的(U,V)(U, V)(U,V)。

由这里的HHH是齐次矩阵，有8个独立未知元素。每一个标定板角点可以提供两个约束方程（u,U,Vu, U, Vu,U,V的对应关系，v,U,Vv, U, Vv,U,V的对应关系提供了两个约束方程）。因此，当一张图片上的标定板角点数量等于4时，即可求得该图片对应的矩阵H。当一张图片上的标定板角点数量大于4时，利用最小二乘法回归最佳的矩阵HHH。

2. 求解内参矩阵

我们已知了矩阵H=A(R1R2T)H = A\left(\begin{matrix} R1 & R2 & T\end{matrix}\right)H=A(R1​R2​T​)，接下来需要求解相机的内参矩阵AAA。

我们利用R1,R2R1, R2R1,R2作为旋转矩阵RRR的两列，存在单位正交的关系，即：

R1TR2=0R1TR1=R2TR2=1R1^TR2 = 0\\ \ \\ R1^TR1 = R2^TR2 = 1 R1TR2=0R1TR1=R2TR2=1

则由HHH和R1,R2R1, R2R1,R2的关系，可知：

R1=A−1H1R2=A−1H2R1 = A^{-1}H1\\ \ \\ R2 = A^{-1}H2 R1=A−1H1R2=A−1H2

代入可得：

H1TA−TA−1H2=0H1TA−TA−1H1=H2TA−TA−1H2=1H1^TA^{-T}A^{-1}H2 = 0\\ \ \\ H1^TA^{-T}A^{-1}H1 = H2^TA^{-T}A^{-1}H2 = 1 H1TA−TA−1H2=0H1TA−TA−1H1=H2TA−TA−1H2=1

另外，我们发现，上述两个约束方程中均存在矩阵A−TA−1A^{-T}A^{-1}A−TA−1。因此，我们记A−TA−1=BA^{-T}A^{-1} = BA−TA−1=B，则BBB为对称阵。我们视图先求解出矩阵BBB，通过矩阵BBB再求解相机的内参矩阵AAA。

同时，为了简便，我们记相机内参矩阵AAA为：

A=(fdX−fcotθdXu000fdYsinθv000010)=[αγu00βv0001]A = \left(\begin{matrix} \frac{f}{dX} & -\frac{fcot\theta}{dX} & u_0 & 0\\ \ \\ 0 & \frac{f}{dYsin\theta} & v_0 & 0\\ \ \\ 0 & 0 & 1 & 0 \end{matrix}\right) = \left[\begin{matrix} \alpha & \gamma & u_0\\ \ \\ 0 & \beta & v_0\\ \ \\ 0 & 0 & 1 \end{matrix}\right] A=⎝⎜⎜⎜⎜⎛​dXf​00​−dXfcotθ​dYsinθf​0​u0​v0​1​000​⎠⎟⎟⎟⎟⎞​=⎣⎢⎢⎢⎢⎡​α00​γβ0​u0​v0​1​⎦⎥⎥⎥⎥⎤​

则：

A−1=[1α−γαβγv0−βu0αβ01β−v0β001]A^{-1} = \left[\begin{matrix} \frac{1}{\alpha} & -\frac{\gamma}{\alpha\beta} & \frac{\gamma v_0 - \beta u_0}{\alpha\beta}\\ \ \\ 0 & \frac{1}{\beta} & -\frac{v_0}{\beta}\\ \ \\ 0 & 0 & 1 \end{matrix}\right] A−1=⎣⎢⎢⎢⎢⎡​α1​00​−αβγ​β1​0​αβγv0​−βu0​​−βv0​​1​⎦⎥⎥⎥⎥⎤​

则用矩阵AAA表示矩阵BBB得：

B=A−TA−1=[1α2−γα2βγv0−βu0α2β−γα2β1β2+γ2α2β2γ(βu0−γv0)α2β2−v0β2γv0−βu0α2βγ(βu0−γv0)α2β2−v0β2(βu0−γv0)2α2β2+v02β2+1]=[B11B12B13B12B22B23B13B23B33]B=A^{-T}A^{-1}= \left[\begin{matrix} \frac{1}{\alpha^2} & -\frac{\gamma}{\alpha^2\beta} & \frac{\gamma v_0 - \beta u_0}{\alpha^2\beta}\\ \ \\ -\frac{\gamma}{\alpha^2\beta} & \frac{1}{\beta^2}+\frac{\gamma^2}{\alpha^2\beta^2} & \frac{\gamma(\beta u_0-\gamma v_0)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2}\\ \ \\ \frac{\gamma v_0-\beta u_0}{\alpha^2\beta} & \frac{\gamma(\beta u_0-\gamma v_0)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2} & \frac{(\beta u_0-\gamma v_0)^2}{\alpha^2\beta^2}+\frac{{v_0}^2}{\beta^2}+1 \end{matrix}\right]= \left[\begin{matrix} B_{11} & B_{12} & B_{13}\\ \ \\ B_{12} & B_{22} & B_{23}\\ \ \\ B_{13} & B_{23} & B_{33} \end{matrix}\right] B=A−TA−1=⎣⎢⎢⎢⎢⎢⎡​α21​−α2βγ​α2βγv0​−βu0​​​−α2βγ​β21​+α2β2γ2​α2β2γ(βu0​−γv0​)​−β2v0​​​α2βγv0​−βu0​​α2β2γ(βu0​−γv0​)​−β2v0​​α2β2(βu0​−γv0​)2​+β2v0​2​+1​⎦⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎡​B11​B12​B13​​B12​B22​B23​​B13​B23​B33​​⎦⎥⎥⎥⎥⎤​

注意：由于BBB为对称阵，上式出现了两次B12,B13,B23B_{12}, B_{13}, B_{23}B12​,B13​,B23​。

这里，我们可以使用B=A−TA−1B = A^{-T}A^{-1}B=A−TA−1将前面通过R1,R2R1, R2R1,R2单位正交得到的约束方程化为：

H1TBH2=0H1TBH1=H2TBH2=1H1^TBH2 = 0\\ \ \\ H1^TBH1 = H2^TBH2 = 1 H1TBH2=0H1TBH1=H2TBH2=1

因此，为了求解矩阵BBB，我们必须计算HiTBHjH_i^TBH_jHiT​BHj​。则：

HiTBHj=[H1iH2iH3i][B11B12B13B12B22B23B13B23B33][H1jH2jH3j]=[H1iH1jH1iH2j+H2iH1jH2iH2jH1iH3j+H3iH1jH2iH3j+H3iH2jH3iH3j][B11B12B13B22B23B33]H_i^TBH_j = \left[\begin{matrix} H_{1i} & H_{2i} & H_{3i}\\ \end{matrix}\right] \left[\begin{matrix} B_{11} & B_{12} & B_{13}\\ \ \\ B_{12} & B_{22} & B_{23}\\ \ \\ B_{13} & B_{23} & B_{33} \end{matrix}\right] \left[\begin{matrix} H_{1j}\\ \ \\ H_{2j}\\ \ \\ H_{3j} \end{matrix}\right]\\ = \left[\begin{matrix} H_{1i}H_{1j} & H_{1i}H_{2j} + H_{2i}H_{1j} & H_{2i}H_{2j} & H_{1i}H_{3j} + H_{3i}H_{1j} & H_{2i}H_{3j} + H_{3i}H_{2j} & H_{3i}H_{3j} \end{matrix}\right] \left[\begin{matrix} B_{11}\\ \ \\ B_{12}\\ \ \\ B_{13}\\ \ \\ B_{22}\\ \ \\ B_{23}\\ \ \\ B_{33} \end{matrix}\right] HiT​BHj​=[H1i​​H2i​​H3i​​]⎣⎢⎢⎢⎢⎡​B11​B12​B13​​B12​B22​B23​​B13​B23​B33​​⎦⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎡​H1j​H2j​H3j​​⎦⎥⎥⎥⎥⎤​=[H1i​H1j​​H1i​H2j​+H2i​H1j​​H2i​H2j​​H1i​H3j​+H3i​H1j​​H2i​H3j​+H3i​H2j​​H3i​H3j​​]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​B11​B12​B13​B22​B23​B33​​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​

上述方程看起来有点复杂，但是其实不然，我们可以记：

vij=[H1iH1jH1iH2j+H2iH1jH2iH2jH1iH3j+H3iH1jH2iH3j+H3iH2jH3iH3j]Tb=[B11B12B13B22B23B33]Tv_{ij} = {\left[\begin{matrix} H_{1i}H_{1j} & H_{1i}H_{2j} + H_{2i}H_{1j} & H_{2i}H_{2j} & H_{1i}H_{3j} + H_{3i}H_{1j} & H_{2i}H_{3j} + H_{3i}H_{2j} & H_{3i}H_{3j} \end{matrix}\right]}^T\\ \ \\ b = {\left[\begin{matrix} B_{11} & B_{12} & B_{13} & B_{22} & B_{23} & B_{33} \end{matrix}\right] }^T vij​=[H1i​H1j​​H1i​H2j​+H2i​H1j​​H2i​H2j​​H1i​H3j​+H3i​H1j​​H2i​H3j​+H3i​H2j​​H3i​H3j​​]Tb=[B11​​B12​​B13​​B22​​B23​​B33​​]T

则上述方程化为：

HiTBHj=vijbH_i^TBH_j = v_{ij}b HiT​BHj​=vij​b

此时，通过R1,R2R1, R2R1,R2单位正交得到的约束方程可化为：

v12Tb=0v11Tb=v22Tb=1v_{12}^Tb = 0\\ \ \\ v_{11}^Tb = v_{22}^Tb = 1 v12T​b=0v11T​b=v22T​b=1

即：

[v12Tv11T−v22T]b=vb=0\left[\begin{matrix} v_{12}^T\\ \ \\ v_{11}^T-v_{22}^T \end{matrix}\right]b=vb=0 ⎣⎡​v12T​v11T​−v22T​​⎦⎤​b=vb=0

其中，矩阵v=[v12Tv11T−v22T]v=\left[\begin{matrix} v_{12}^T\\ \ \\ v_{11}^T-v_{22}^T \end{matrix}\right]v=⎣⎡​v12T​v11T​−v22T​​⎦⎤​

由于矩阵HHH已知，矩阵vvv又全部由矩阵HHH的元素构成，因此矩阵vvv已知。

此时，我们只要求解出向量bbb，即可得到矩阵BBB。每张标定板图片可以提供一个vb=0vb=0vb=0的约束关系，该约束关系含有两个约束方程。但是，向量bbb有6个未知元素。因此，单张图片提供的两个约束方程是不足以解出来向量bbb。因此，我们只要取3张标定板照片，得到3个vb=0vb=0vb=0的约束关系，即6个方程，即可求解向量bbb。当标定板图片的个数大于3时（事实上一般需要15到20张标定板图片），可采用最小二成拟合最佳的向量bbb，并得到矩阵BBB。

B=[1α2−γα2βγv0−βu0α2β−γα2β1β2+γ2α2β2γ(βu0−γv0)α2β2−v0β2γv0−βu0α2βγ(βu0−γv0)α2β2−v0β2(βu0−γv0)2α2β2+v02β2+1]=[B11B12B13B12B22B23B13B23B33]B= \left[\begin{matrix} \frac{1}{\alpha^2} & -\frac{\gamma}{\alpha^2\beta} & \frac{\gamma v_0 - \beta u_0}{\alpha^2\beta}\\ \ \\ -\frac{\gamma}{\alpha^2\beta} & \frac{1}{\beta^2}+\frac{\gamma^2}{\alpha^2\beta^2} & \frac{\gamma(\beta u_0-\gamma v_0)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2}\\ \ \\ \frac{\gamma v_0-\beta u_0}{\alpha^2\beta} & \frac{\gamma(\beta u_0-\gamma v_0)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2} & \frac{(\beta u_0-\gamma v_0)^2}{\alpha^2\beta^2}+\frac{{v_0}^2}{\beta^2}+1 \end{matrix}\right]= \left[\begin{matrix} B_{11} & B_{12} & B_{13}\\ \ \\ B_{12} & B_{22} & B_{23}\\ \ \\ B_{13} & B_{23} & B_{33} \end{matrix}\right] B=⎣⎢⎢⎢⎢⎢⎡​α21​−α2βγ​α2βγv0​−βu0​​​−α2βγ​β21​+α2β2γ2​α2β2γ(βu0​−γv0​)​−β2v0​​​α2βγv0​−βu0​​α2β2γ(βu0​−γv0​)​−β2v0​​α2β2(βu0​−γv0​)2​+β2v0​2​+1​⎦⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎡​B11​B12​B13​​B12​B22​B23​​B13​B23​B33​​⎦⎥⎥⎥⎥⎤​

根据矩阵BBB的元素和相机内参α,β,γ,u0,v0\alpha, \beta, \gamma, u_0, v_0α,β,γ,u0​,v0​的对应关系（如上式），可得到：

v0=B12B13−B11B23B11B22−B122α=1B11β=B11B11B22−B122γ=−B12α2βu0=γv0β−B13α2v_0 = \frac{B_{12}B_{13}-B_{11}B_{23}}{B_{11}B_{22}-B_{12}^2}\\ \ \\ \alpha = \sqrt{\frac{1}{B_{11}}}\\ \ \\ \beta = \sqrt{\frac{B_{11}}{B_{11}B_{22}-B_{12}^2}}\\ \ \\ \gamma = -B_{12}\alpha^2\beta\\ \ \\ u_0 = \frac{\gamma v_0}{\beta}-B_{13}\alpha^2 v0​=B11​B22​−B122​B12​B13​−B11​B23​​α=B11​1​​β=B11​B22​−B122​B11​​​γ=−B12​α2βu0​=βγv0​​−B13​α2

即可求得相机的内参矩阵A=(fdX−fcotθdXu000fdYsinθv000010)=[αγu00βv0001]A=\left(\begin{matrix} \frac{f}{dX} & -\frac{fcot\theta}{dX} & u_0 & 0\\ \ \\ 0 & \frac{f}{dYsin\theta} & v_0 & 0\\ \ \\ 0 & 0 & 1 & 0 \end{matrix}\right) = \left[\begin{matrix} \alpha & \gamma & u_0\\ \ \\ 0 & \beta & v_0\\ \ \\ 0 & 0 & 1 \end{matrix}\right]A=⎝⎜⎜⎜⎜⎛​dXf​00​−dXfcotθ​dYsinθf​0​u0​v0​1​000​⎠⎟⎟⎟⎟⎞​=⎣⎢⎢⎢⎢⎡​α00​γβ0​u0​v0​1​⎦⎥⎥⎥⎥⎤​

3. 求解外参矩阵

这里再次强调一下，对于同一个相机，相机的内参矩阵取决于相机的内部参数，无论标定板和相机的位置关系是怎么样的，相机的内参矩阵不变。这也正是在第2部分“求解内参矩阵”中，我们可以利用不同的图片（标定板和相机位置关系不同）获取的矩阵HHH，共同求解相机内参矩阵AAA的原因。

但是，外参矩阵反映的是标定板和相机的位置关系。对于不同的图片，标定板和相机的位置关系已经改变，此时每一张图片对应的外参矩阵都是不同的。

在关系：A(R1R2T)=HA\left(\begin{matrix} R1 & R2 & T\end{matrix}\right) = HA(R1​R2​T​)=H中，我们已经求解得到了矩阵HHH（对于同一张图片相同，对于不同的图片不同）、矩阵AAA（对于不同的图片都相同）。通过公式：(R1R2T)=A−1H\left(\begin{matrix} R1 & R2 & T\end{matrix}\right)=A^{-1}H(R1​R2​T​)=A−1H，即可求得每一张图片对应的外参矩阵(R1R2T)\left(\begin{matrix} R1 & R2 & T\end{matrix}\right)(R1​R2​T​)。

注意，这里值得指出，完整的外参矩阵为(RT01)\left(\begin{matrix} R & T\\ 0 & 1\end{matrix}\right)(R0​T1​)。但是，由于张正友标定板将世界坐标系的原点选取在棋盘格上，则棋盘格上任一点的物理坐标W=0W=0W=0，将旋转矩阵的RRR的第三列R3R3R3消掉。因此，R3R3R3在坐标转化中并没有作用。但是R3R3R3要使得RRR满足旋转矩阵的性质，即列与列之间单位正交。因此可以通过向量R1,R2R1, R2R1,R2的叉乘，即R3=R1×R2R3=R1\times R2R3=R1×R2，计算得到R3R3R3。

此时，相机的内参矩阵和外参矩阵均已得到。

注：以上推导都是假设不存在畸变参数的情况下成立的。但是事实上，相机是存在畸变参数的，因此张正友标定法还需要通过L-M算法对于参数进行迭代优化。

四、标定相机的畸变参数

张正友标定法仅仅考虑了畸变模型中影响较大的径向畸变。

径向畸变公式（2阶）如下：

x^=x(1+k1r2+k2r4)y^=y(1+k1r2+k2r4)\hat{x} = x(1 + k_1r^2 + k_2r^4) \\ \ \\ \hat{y} = y(1 + k_1r^2 + k_2r^4) x^=x(1+k1​r2+k2​r4)y^​=y(1+k1​r2+k2​r4)

其中，(x,y),(x^,y^)(x, y), (\hat{x}, \hat{y})(x,y),(x^,y^​)分别为理想的无畸变的归一化的图像坐标、畸变后的归一化图像坐标，rrr为图像像素点到图像中心点的距离，即r2=x2+y2r^2 = x^2 + y^2r2=x2+y2。

图像坐标和像素坐标的转化关系为：

(uv1)=(1dX−cotθdXu001dYsinθv0001)(xy1)\left(\begin{matrix} u\\ v\\ 1 \end{matrix}\right) = \left(\begin{matrix} \frac{1}{dX} & -\frac{cot\theta}{dX} & u_0\\ \ \\ 0 & \frac{1}{dYsin\theta} & v_0\\ \ \\ 0 & 0 & 1 \end{matrix}\right) \left(\begin{matrix} x\\ y\\ 1 \end{matrix}\right) ⎝⎛​uv1​⎠⎞​=⎝⎜⎜⎜⎜⎛​dX1​00​−dXcotθ​dYsinθ1​0​u0​v0​1​⎠⎟⎟⎟⎟⎞​⎝⎛​xy1​⎠⎞​

其中，(u,v)(u, v)(u,v)为理想的无畸变的像素坐标。由于θ\thetaθ接近于90°，则上式近似为：

u=xdX+u0v=ydY+v0u = \frac{x}{dX} + u_0\\ \ \\ v = \frac{y}{dY} + v_0 u=dXx​+u0​v=dYy​+v0​

同理可得畸变后的像素坐标(u^,v^)(\hat{u}, \hat{v})(u^,v^)的表达式为：

u^=x^dX+u0v^=y^dY+v0\hat{u} = \frac{\hat{x}}{dX} + u_0\\ \ \\ \hat{v} = \frac{\hat{y}}{dY} + v_0 u^=dXx^​+u0​v^=dYy^​​+v0​

代入径向畸变公式（2阶）则有：

u^−u0=(u−u0)(1+k1r2+k2r4)v^−v0=(v−v0)(1+k1r2+k2r4)\hat{u}-u_0 = (u-u_0)(1 + k_1r^2 + k_2r^4) \\ \ \\ \hat{v}-v_0 = (v-v_0)(1 + k_1r^2 + k_2r^4) u^−u0​=(u−u0​)(1+k1​r2+k2​r4)v^−v0​=(v−v0​)(1+k1​r2+k2​r4)

可化简得：

u^=u+(u−u0)(k1r2+k2r4)v^=v+(v−v0)(k1r2+k2r4)\hat{u} = u+(u-u_0)(k_1r^2 + k_2r^4) \\ \ \\ \hat{v} = v+(v-v_0)(k_1r^2 + k_2r^4) u^=u+(u−u0​)(k1​r2+k2​r4)v^=v+(v−v0​)(k1​r2+k2​r4)

即为：

[(u−u0)r2(u−u0)r4(v−v0)r2(v−v0)r4][k1k2]=[u^−uv^−v]\left[ \begin{matrix} (u-u_0)r^2 & (u-u_0)r^4\\ \ \\ (v-v_0)r^2 & (v-v_0)r^4 \end{matrix} \right] \left[ \begin{matrix} k_1\\ \ \\ k_2 \end{matrix} \right]= \left[ \begin{matrix} \hat{u}-u\\ \ \\ \hat{v}-v \end{matrix} \right] ⎣⎡​(u−u0​)r2(v−v0​)r2​(u−u0​)r4(v−v0​)r4​⎦⎤​⎣⎡​k1​k2​​⎦⎤​=⎣⎡​u^−uv^−v​⎦⎤​

每一个角点，只要知道畸变后的像素坐标(u^,v^)(\hat{u}, \hat{v})(u^,v^)、理想的无畸变的像素坐标(u,v)(u, v)(u,v)，就可以构造两个上述等式。那么，有mmm幅图像，每幅图像上有nnn个标定板角点，则将得到的所有等式组合起来，可以得到mnmnmn个未知数为k=[k1,k2]Tk=[k_1, k_2]^Tk=[k1​,k2​]T的约束方程，将约束方程系数矩阵记为DDD，等式右端非齐次项记为ddd，可将其记为矩阵形式：

Dk=dDk=dDk=d

之后，利用最小二乘法可得：

k=[k1k2]=(DTD)−1DTdk=\left[\begin{matrix}k_1\\ \ \\ k_2 \end{matrix}\right] =(D^TD)^{-1}D^Td k=⎣⎡​k1​k2​​⎦⎤​=(DTD)−1DTd

此时，相机畸变矫正参数已经标定好。

那么，如何获得畸变后的像素坐标(u^,v^)(\hat{u}, \hat{v})(u^,v^)和理想的无畸变的像素坐标(u,v)(u, v)(u,v)呢？

(u^,v^)(\hat{u}, \hat{v})(u^,v^)可以通过识别标定板的角点获得，(u,v)(u, v)(u,v)可以通过如下方法近似求得。世界坐标系下的每一个角点的坐标(U,V)(U, V)(U,V)是可以计算得到的，我们利用已经求得的外参矩阵(R1R2T)\left(\begin{matrix} R1 & R2 & T\end{matrix}\right)(R1​R2​T​)和内参矩阵AAA进行反投影。

Z(uv1)=A(R1R2T)(UV1)=H(UV1)Z\left(\begin{matrix} u\\ \ \\ v\\ \ \\ 1 \end{matrix}\right) =A \left(\begin{matrix} R1 & R2 & T\end{matrix}\right) \left(\begin{matrix} U\\ \ \\ V\\ \ \\ 1 \end{matrix}\right) = H \left(\begin{matrix} U\\ \ \\ V\\ \ \\ 1 \end{matrix}\right) Z⎝⎜⎜⎜⎜⎛​uv1​⎠⎟⎟⎟⎟⎞​=A(R1​R2​T​)⎝⎜⎜⎜⎜⎛​UV1​⎠⎟⎟⎟⎟⎞​=H⎝⎜⎜⎜⎜⎛​UV1​⎠⎟⎟⎟⎟⎞​

利用上式，消去尺度因子ZZZ，可得：

u=H11U+H12V+H13H31U+H32V+H33v=H21U+H22V+H23H31U+H32V+H33u = \frac{H_{11}U+H_{12}V+H_{13}}{H_{31}U+H_{32}V+H_{33}}\\ \ \\ v = \frac{H_{21}U+H_{22}V+H_{23}}{H_{31}U+H_{32}V+H_{33}} u=H31​U+H32​V+H33​H11​U+H12​V+H13​​v=H31​U+H32​V+H33​H21​U+H22​V+H23​​

即可得到理想的、无畸变的像素坐标(u,v)(u, v)(u,v)。当然，由于外参矩阵(R1R2T)\left(\begin{matrix} R1 & R2 & T\end{matrix}\right)(R1​R2​T​)和内参矩阵AAA是在有畸变的情况下获得的，这里得到的像素坐标(u,v)(u, v)(u,v)并不是完全理想的、无畸变的。我们的总逻辑是，在进行内参矩阵和外参矩阵的求解的时候，我们假设不存在畸变；在进行畸变系数的求解的时候，我们假设求得的内参矩阵和外参矩阵都是无误差的。最后，我们再通过L-M算法对于参数进行迭代优化。

需要指出，上述公式推导的时候以2阶径向畸变为例，但实际上更高阶的径向畸变同理，只是需要的约束方程个数更多而已。

注意：

在DDD矩阵的构建过程中，需要用到r2=x2+y2r^2=x^2+y^2r2=x2+y2。而由于张正友标定法不能直接求出焦距fff，理想的无畸变的归一化的图像坐标(x,y)(x, y)(x,y)无法求解，造成DDD矩阵无法构建的问题。

以下方案可以作为一种解决方案。

世界坐标系下的标定板角点的坐标(U,V)(U, V)(U,V)乘上刚体变换矩阵（外参矩阵）即可转化为相机坐标系下的标定板角点坐标(X,Y,Z)(X, Y, Z)(X,Y,Z)。

(XYZ)=(R1R2T)(UV1)\left(\begin{matrix} X\\ \ \\ Y\\ \ \\ Z \end{matrix}\right) = \left(\begin{matrix} R1 & R2 & T\end{matrix}\right) \left(\begin{matrix} U\\ \ \\ V\\ \ \\ 1 \end{matrix}\right) ⎝⎜⎜⎜⎜⎛​XYZ​⎠⎟⎟⎟⎟⎞​=(R1​R2​T​)⎝⎜⎜⎜⎜⎛​UV1​⎠⎟⎟⎟⎟⎞​

此时，相机坐标系下的标定板角点坐标(X,Y,Z)(X, Y, Z)(X,Y,Z)乘上透视投影矩阵可得：

Z(xy1)=(f0000f000010)(XYZ)Z\left(\begin{matrix} x\\ \ \\ y\\ \ \\ 1 \end{matrix}\right) = \left(\begin{matrix} f & 0 & 0 & 0\\ \ \\ 0 & f & 0 & 0\\ \ \\ 0 & 0 & 1 & 0 \end{matrix}\right) \left(\begin{matrix} X\\ \ \\ Y\\ \ \\ Z \end{matrix}\right) Z⎝⎜⎜⎜⎜⎛​xy1​⎠⎟⎟⎟⎟⎞​=⎝⎜⎜⎜⎜⎛​f00​0f0​001​000​⎠⎟⎟⎟⎟⎞​⎝⎜⎜⎜⎜⎛​XYZ​⎠⎟⎟⎟⎟⎞​

其中，(x,y)(x, y)(x,y)为理想的无畸变的归一化的图像坐标。即为：

x=fXZy=fYZx = \frac{fX}{Z}\\ \ \\ y = \frac{fY}{Z} x=ZfX​y=ZfY​

记R2=X2+Y2R^2=X^2+Y^2R2=X2+Y2，则有r2=x2+y2=f2R2Z2r^2=x^2+y^2=\frac{f^2R^2}{Z^2}r2=x2+y2=Z2f2R2​。

代入Dk=dDk=dDk=d中，可得：

[(u−u0)R2Z2(u−u0)R4Z4(v−v0)R2Z2(v−v0)R4Z4][f2k1f4k2]=[u^−uv^−v]\left[ \begin{matrix} (u-u_0)\frac{R^2}{Z^2} & (u-u_0)\frac{R^4}{Z^4}\\ \ \\ (v-v_0)\frac{R^2}{Z^2} & (v-v_0)\frac{R^4}{Z^4} \end{matrix} \right] \left[ \begin{matrix} f^2k_1\\ \ \\ f^4k_2 \end{matrix} \right]= \left[ \begin{matrix} \hat{u}-u\\ \ \\ \hat{v}-v \end{matrix} \right] ⎣⎡​(u−u0​)Z2R2​(v−v0​)Z2R2​​(u−u0​)Z4R4​(v−v0​)Z4R4​​⎦⎤​⎣⎡​f2k1​f4k2​​⎦⎤​=⎣⎡​u^−uv^−v​⎦⎤​

我们将上式重新记为D′k′=dD\prime k\prime=dD′k′=d，此时这个系数矩阵D′D\primeD′是可以完全求出来的，利用最小二乘法求解k′k\primek′为：

k′=[k1′k2′]=[f2k1f2k2]=(D′TD′)−1D′Tdk\prime = \left[ \begin{matrix} k_1\prime\\ \ \\ k_2\prime \end{matrix} \right] = \left[ \begin{matrix} f^2k_1\\ \ \\ f^2k_2 \end{matrix} \right] = (D\prime^TD\prime)^{-1}D\prime^Td k′=⎣⎡​k1​′k2​′​⎦⎤​=⎣⎡​f2k1​f2k2​​⎦⎤​=(D′TD′)−1D′Td

这里解得的k′k\primek′虽然不是真正的畸变系数，但是由于焦距fff是定值，因此k′k\primek′也是定值，当求得k′k\primek′之后，可以将畸变模型化为：

u^−u0=(u−u0)(1+k1′R2Z2+k2′R4Z4)v^−v0=(v−v0)(1+k1′R2Z2+k2′R4Z4)\hat{u}-u_0 = (u-u_0)(1+k_1\prime\frac{R^2}{Z^2}+k_2\prime\frac{R^4}{Z^4})\\ \ \\ \hat{v}-v_0 = (v-v_0)(1+k_1\prime\frac{R^2}{Z^2}+k_2\prime\frac{R^4}{Z^4}) u^−u0​=(u−u0​)(1+k1​′Z2R2​+k2​′Z4R4​)v^−v0​=(v−v0​)(1+k1​′Z2R2​+k2​′Z4R4​)

此时可以直接在像素坐标系下对畸变参数进行矫正。

五、L-M算法参数优化

从上述推导过程就可以看出，张正友标定法是有很多近似的，所以仅仅利用上述的过程进行标定误差肯定是很大的。所以张正友标定法还利用L-M（Levenberg-Marquardt）算法对参数进行了优化。

六、相机标定的步骤

准备一个张正友标定法的棋盘格，棋盘格大小已知，用相机对其进行不同角度的拍摄，得到一组图像；

对图像中的特征点如标定板角点进行检测，得到标定板角点的像素坐标值，根据已知的棋盘格大小和世界坐标系原点，计算得到标定板角点的物理坐标值；

求解内参矩阵与外参矩阵；

根据物理坐标值和像素坐标值的关系，求出HHH矩阵，进而构造vvv矩阵，求解BBB矩阵，利用BBB矩阵求解相机内参矩阵AAA，最后求解每张图片对应的相机外参矩阵(RT01)\left(\begin{matrix} R & T\\ 0 & 1\end{matrix}\right)(R0​T1​)；

求解畸变参数；

利用u^,u,v^,v\hat{u}, u, \hat{v}, vu^,u,v^,v构造DDD矩阵，计算径向畸变参数；

利用L-M（Levenberg-Marquardt）算法对上述参数进行优化。

七、源代码

python基于opencv的源代码：

Calibration_ZhangZhengyou_Method