opencv相机标定原理

c++ opencv相机标定原理

相机标定（相机校准）

真实物体通过相机在图像中显示。可以通过一个数学模型将真实物体的三维坐标与图像中的二维坐标一一对应，这个数学模型（相机矩阵）的求取计算过程就是相机标定。

从三维坐标（世界坐标系）到二维坐标（图像坐标系）又可以分为三个步骤：

（1）从世界坐标转换到相机坐标；

（2）从相机坐标转换到图像坐标；

（3）从图像坐标转换到像素坐标。

四个坐标系之间的关系图

（1）坐标系（Xw, Yw, Zw）为世界坐标系；

（2）坐标系（Xc, Yc, Zc）为相机坐标系；

（3）坐标系（x, y）为图像坐标系；

（4）坐标系（u, v）为像素坐标系；

从世界坐标转换到相机坐标

点P从世界坐标（Xpw, Ypw, Zpw）到相机坐标（Xpc, Ypc, Zpc）的变换包含了一个三维的旋转变换R以及一个平移变换t，齐次表达式为：
$$\left[\begin{array}{c}Xpc\\ Ypc\\ Zpc\end{array}\right]=[R\mid t]\left[\begin{array}{c}Xpw\\ Ypw\\ Zpw\\ 1\end{array}\right]$$
其中R为3×3大小的旋转矩阵，t为3×1的平移矩阵，变换矩阵P为3×4大小的矩阵：
$$P=[R\mid t]$$

从相机坐标转换到图像坐标

根据小孔成像模型，则由P的相机坐标（Xpc, Ypc, Zpc）到图像坐标（xp, yp）有：
$$\begin{array}{rl}& y_p=f_y\times \frac{Ypc}{Zpc}\\ & x_p=f_x\times \frac{Xpc}{Zpc}\end{array}$$
转换为矩阵为：
$$\left[\begin{array}{c}{x}_p\\ y_p\\ 1\end{array}\right]=\frac{1}{Z_{pc}}\left[\begin{array}{ccc}{f}_x& 0& 0\\ 0& f_y& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_{pc}\\ Y_{pc}\\ Z_{pc}\end{array}\right]$$
fx, fy分别为x, y方向上的焦距。

从图像坐标转换到像素坐标

图像坐标系平面可以认为是理想的成像面，像素坐标系则是相机传感器平面，理想情况下图像坐标平面和像素坐标平面在同一平面，则从图像坐标转换到像素坐标有两个步骤：

（1）缩放，在前面的坐标系中，坐标系单位是统一的实际距离单位，但是像素坐标系的单位不一样，因此要缩放，换算到像素坐标系；

（2）平移，从图像坐标系原点到像素坐标系原点。

则变换为：
$$\left[\begin{array}{c}{u}_p\\ v_p\\ 1\end{array}\right]=\left[\begin{array}{ccc}{s}_x& 0& c_x\\ 0& s_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_p\\ y_p\\ 1\end{array}\right]$$
其中sx和sy为单位长度的像素数。

以上是理论情况下，但是在实际中可能由于相机传感器的问题导致图像轴不垂直的情况，如下图

像素偏斜

可以认为在y方向坐标不变，x方向坐标存在偏移，则有
$$\left[\begin{array}{c}{u}_p\\ v_p\\ 1\end{array}\right]=\left[\begin{array}{ccc}{s}_x& c& c_x\\ 0& s_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_p\\ y_p\\ 1\end{array}\right]$$
其中c为偏移系数。

相机内参、外参

由1.1, 1.2, 1.3小结，得到最终的由世界坐标到像素坐标转换关系：
$$\left[\begin{array}{c}{u}_p\\ v_p\\ 1\end{array}\right]=\left[\begin{array}{ccc}{s}_x& c& c_x\\ 0& s_y& c_y\\ 0& 0& 1\end{array}\right]\frac{1}{Z_{pc}}\left[\begin{array}{ccc}{f}_x& 0& 0\\ 0& f_y& 0\\ 0& 0& 1\end{array}\right][R\mid t]\left[\begin{array}{c}Xpw\\ Ypw\\ Zpw\\ 1\end{array}\right]$$
即：
$$s\left[\begin{array}{c}{u}_p\\ v_p\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x{s}_x& f_{y}c& cx\\ 0& f_y{s}_y& cy\\ 0& 0& 1\end{array}\right][R\mid t]\left[\begin{array}{c}Xpw\\ Ypw\\ Zpw\\ 1\end{array}\right]$$
得到：
$$\begin{gathered} s\tilde{m}=\mathbf{A}[\mathbf{R}\mid \mathbf{t}]\tilde{\mathbf{M}} \\ 其中： \\ s=Z_{pc}为尺度参数 \\ A是相机内参，R，t是相机外参 \\ \tilde{m}为图像的齐次坐标[u,v,1{]}^T \\ \tilde{\mathbf{M}}为模型点的齐次坐标[X_{pw},Y_{pw},Z_{pw},1{]}^T \\ A=\left[\begin{array}{ccc}{f}_x{s}_x& f_{y}c& cx\\ 0& f_y{s}_y& cy\\ 0& 0& 1\end{array}\right] \end{gathered}$$

镜头畸变系数

除了相机硬件的参数外，镜头也不会是理想中的完美镜头，由于镜头的缺陷也会导致坐标换算出现误差，因此需要考虑镜头带来的图像失真。镜头带来的畸变包括径向畸变和切向畸变两种。

径向畸变

当光线在镜头边缘附近比在其光学中心弯曲得更多时，就会发生径向畸变。镜头越小，畸变越大。包含枕形（pincushion）、桶形（barrel）两种：

径向畸变通过径向畸变系数模型表示并校正：
$$\begin{array}{rl}& x_{\text{畸变 }}=x\left(1+k_1\ast r^2+k_2\ast r^4+k_3\ast r^6\right)\\ & y_{\text{畸变 }}=y\left(1+k_1\ast r^2+k_2\ast r^4+k_3\ast r^6\right)\end{array}$$
其中：

x, y为未发生畸变的像素位置；

k1, k2, k3为径向畸变系数，一般两个系数就足够校准，但是对于大畸变如广角镜头就需要三个系数；

r^2 = x^2 + y^2.

切向畸变

当镜头和像平面不平行时会发生切向畸变：

切向畸变通过切向畸变系数模型表示并校正：

$$\begin{array}{c}{x}_{\text{畸变 }}=x+\left[2\ast p_1\ast x\ast y+p_2\ast \left(r^2+2\ast x^2\right)\right]\\ y_{\text{畸变 }}=y+\left[p_1\ast \left(r^2+2\ast y^2\right)+2\ast p_2\ast x\ast y\right]\end{array}$$
x, y为未发生畸变的像素位置；

k1, k2为切向畸变系数；

r^2 = x^2 + y^2.

小结

相机标定主要计算：

（1）相机外参：世界坐标系到相机坐标系的变换矩阵（三轴旋转角度+三轴平移量总共6个参数）

（2）相机内参：焦距（fx, fy），光心/主点（cx, cy），倾斜系数（c）（5个参数）

（3）镜头畸变系数：径向畸变系数和切向畸变系数（k1, k2, p1, p2，4个参数）

张正友标定法

单应性、单应性矩阵、齐次坐标

单应性：平面的单应性被定义为从一个平面到另一个平面的投影映射，单应性变换或投影变换；

单应性矩阵：两个投影平面的变换矩阵，投影变换矩阵；

齐次坐标：类似定义三维空间坐标系的笛卡尔坐标系，齐次坐标系定义投影空间，齐次坐标与笛卡尔坐标转换关系如下：

二维平面：
$$\begin{gathered} 笛卡尔坐标:(x,y) \\ 齐次坐标:\left(x^{\prime },y^{\prime },w^{\prime }\right) \\ 则有: \\ \begin{array}{rl}& x=\frac{x}{w}\\ & y=\frac{y}{w}\end{array} \end{gathered}$$

三维空间：
$$\begin{gathered} 笛卡尔坐标:(x,y,z) \\ 齐次坐标:\left(x^{\prime },y^{\prime },z^{\prime },w^{\prime }\right) \\ 则有: \\ \begin{array}{rl}& x=\frac{x}{w}\\ & y=\frac{y^{\prime }}{w}\\ & z=\frac{z^{\prime }}{w}\end{array} \end{gathered}$$

张正友标定法数学模型

基本方程

在不考虑镜头畸变时，模型点到图像点的变换关系表示为：
$$s\tilde{m}=\mathbf{A}[\mathbf{R}\mid \mathbf{t}]\tilde{\mathbf{M}}\text{\hspace{1em}(1)}$$
其中：
$$\begin{gathered} A是相机内参矩阵,\mathbf{R},t是相机外参,s为尺度系数。 \\ \tilde{m}为图像的齐次坐标[u,v,1{]}^T \\ \tilde{M}为模型点的齐次坐标[X,Y,Z,1{]}^T \end{gathered}$$
假设模型平面在世界坐标系的Z = 0（标定时将棋盘格作为世界坐标系原点，则Z=0）上。让我们用ri表示旋转矩阵R的第i列,则由（1）有：
$$\begin{array}{rl}& s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\mathbf{A}[\mathbf{r}1,r2,\mathbf{r}3,\mathbf{t}]\left[\begin{array}{c}X\\ Y\\ 0\\ 1\end{array}\right]=\mathbf{A}[r1,r2,\mathbf{t}]\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]\\ & \Rightarrow s\tilde{m}=\mathbf{H}\tilde{\mathbf{M}}\end{array}\text{\hspace{1em}(2)}$$
其中：
$$\mathbf{H}=\mathbf{A}[r1,r2,\mathbf{t}]$$
H是一个3×3的系数矩阵（单应性矩阵）：
$$\mathbf{H}=\mathbf{A}[\mathbf{r}1,\mathbf{r}2,\mathbf{t}]=\left[\begin{array}{lll}h11& h12& h13\\ h21& h22& h23\\ h31& h32& h33\end{array}\right]\text{\hspace{1em}(3)}$$
对于齐次坐标系中的H有性质：

这里使用的是齐次坐标系，也就是说可以进行任意尺度的缩放（s为尺度因子），也就是说把H乘以任意一个非零常数k并不改变上式结果，无非就是尺度因子s有所改变。

例如将H乘以1/h33：
$$H^{\prime }=\frac{1}{h33}H=\left[\begin{array}{ccc}h11^{\prime }& h12^{\prime }& h13^{\prime }\\ h21^{\prime }& h22^{\prime }& h23^{\prime }\\ h31^{\prime }& h32^{\prime }& 1\end{array}\right]$$
H’与H带入公式（2）得到的计算结果u, v相等。因此可以认为H只有8个自由度。

内在参数的约束条件

令 H = [h1 h2 h3]，则由公式（3）有：
$$\left[\begin{array}{lll}h1& h2& h3\end{array}\right]=kA\left[\begin{array}{lll}r1& r2& t\end{array}\right]\text{\hspace{1em}(4)}$$
其中k为任意的尺度因子。由于旋转矩阵是一个标准的正交矩形，因此r1和r2为正交的单位向量，有：
$$\begin{array}{rl}& r_1^T{r}_2=0\\ & r_1^T{r}_1=r_2^T{r}_2=1\end{array}\text{\hspace{1em}(5)}$$
有公式（4）有：
$$\begin{array}{rl}& h1=kA{r}_1\\ & h2=kA{r}_2\end{array}\text{\hspace{1em}(6)}$$
进一步由公式（6）得到：
$$\begin{array}{rl}& r_1=k^{-1}{A}^{-1}h1\\ & r_2=k^{-1}{A}^{-1}h2\end{array}\text{\hspace{1em}(7)}$$
结合公式（5）和（7）有：
$$\begin{array}{rl}& r_1^T{r}_2={\left(k^{-1}{A}^{-1}h1\right)}^T{k}^{-1}{A}^{-1}h2=0\\ & \Rightarrow h{1}^T{A}^{-T}{A}^{-1}h2=0\end{array}\text{\hspace{1em}(8)}$$

$$\begin{array}{rl}& r_1^T{r}_1=r_2^T{r}_2=1\\ & \Rightarrow {\left(k^{-1}{A}^{-1}h1\right)}^T{k}^{-1}{A}^{-1}h1={\left(k^{-1}{A}^{-1}h2\right)}^T{k}^{-1}{A}^{-1}h2\\ & \Rightarrow h{1}^T{A}^{-T}{A}^{-1}h1=h{2}^T{A}^{-T}{A}^{-1}h2\end{array}\text{\hspace{1em}(9)}$$

得到（8）（9）两个约束。

参数求解

参数求解过程：

（1）给出一个封闭解

（2）根据最大似然估计给出非线性的最优化解

（3）考虑镜头的径向畸变（张正友标定法不考虑切向畸变），给出解析解和非线性解

封闭解

有：
$$A=\left[\begin{array}{ccc}\alpha & c& u_0\\ 0& \beta & v_0\\ 0& 0& 1\end{array}\right]$$
令：
$$B=A^{-T}{A}^{-1}=\left[\begin{array}{ccc}\frac{1}{{\alpha }^2}& \frac{-c}{{\alpha }^2\beta }& \frac{c{v}_0-u_0\beta }{{\alpha }^2\beta }\\ \frac{-c}{{\alpha }^2\beta }& \frac{c}{{\alpha }^2{\beta }^2}+\frac{1}{{\beta }^2}& -\frac{c\left(c{v}_0-u_0\beta \right)}{{\alpha }^2{\beta }^2}-\frac{v_0}{{\beta }^2}\\ \frac{c{v}_0-u_0\beta }{{\alpha }^2\beta }& -\frac{c\left(c{v}_0-u_0\beta \right)}{{\alpha }^2{\beta }^2}-\frac{v_0}{{\beta }^2}& \frac{{\left(c{v}_0-u_0\beta \right)}^2}{{\alpha }^2{\beta }^2}+\frac{v_0^2}{{\beta }^2}+1\end{array}\right]\text{\hspace{1em}(10)}$$
其中B是一个对称矩阵，设一个六维向量b：
$$b=[B11,B12,B13,B22,B23,B33{]}^T\text{\hspace{1em}(11)}$$
设H的第i列向量为
$$h_i={\left[h_{i1},h_{i2},h_{i3}\right]}^T$$
有：
$$h_i^{T}B{h}_j=v_{ij}^{T}b\text{\hspace{1em}(12)}$$
其中：
$$v_{ij}=\left[\begin{array}{c}{h}_{i1}{h}_{j1}\\ h_{i1}{h}_{j2}+h_{i2}{h}_{j1}\\ h_{i2}{h}_{j2}\\ h_{i3}{h}_{j1}+h_{i1}{h}_{j3}\\ h_{i3}{h}_{j2}+h_{i2}{h}_{j3}\\ h_{i3}{h}_{j3}\end{array}\right]$$
约束公式（8）（9）可以重写为：
$$\begin{array}{rl}& r_1^T{r}_2={\left(k^{-1}{A}^{-1}h1\right)}^T{k}^{-1}{A}^{-1}h2=0\\ & \Rightarrow h{1}^T{A}^{-T}{A}^{-1}h2=0\\ & \Rightarrow h{1}^{T}Bh2=0\\ & \Rightarrow v_{12}^{T}b=0\end{array}$$

$$\begin{array}{rl}& r_1^T{r}_1=r_2^T{r}_2=1\\ & \Rightarrow {\left(k^{-1}{A}^{-1}h1\right)}^T{k}^{-1}{A}^{-1}h1={\left(k^{-1}{A}^{-1}h2\right)}^T{k}^{-1}{A}^{-1}h2\\ & \Rightarrow h{1}^T{A}^{-T}{A}^{-1}h1=h{2}^T{A}^{-T}{A}^{-1}h2\\ & \Rightarrow h{1}^{T}Bh1=h{2}^{T}Bh2\\ & \Rightarrow {\left(v_{11}-v_{22}\right)}^{T}b=0\end{array}$$

由上面两式得到：
$$\left[\begin{array}{c}{v}_{12}^T\\ {\left(v_{11}-v_{22}\right)}^T\end{array}\right]b=0\text{\hspace{1em}(13)}$$
公式（13）为一个给定单应性中的两个基本约束条件，即标定一张图像就给出上面两个约束。

通过标定n张图像，则有n个公式（13），叠加表示得到： $Vb=0$ 其中V是一个2n*6的矩阵。

因为b是一个6维向量，因此最少有6个方程就可以求解这6个未知量，每张图像可以提供两个约束方程。若有3张图像则可以解出给定比例因子的6维向量b的解。

若只有2张图像，可以施加无倾斜系数的约束，即c=0，有： $[0,1,0,0,0,0]b=0$ 将上式代入（13）作为额外约束。

得到b的解后，即得到B，并解出相机内参A。解出A，则能够解出相机外参。

得到B后。相机内参A计算如下：
$$\begin{array}{rl}& v_0=\left(B_{12}{B}_{13}-B_{11}{B}_{23}\right)/\left(B_{11}{B}_{22}-B_{12}^2\right)\\ & \lambda =B_{33}-\left[B_{13}^2+v_0\left(B_{12}{B}_{13}-B_{11}{B}_{23}\right)\right]/B_{11}\\ & \alpha =\sqrt{\lambda /B_{11}}\\ & \beta =\sqrt{\lambda B_{11}/\left(B_{11}{B}_{22}-B_{12}^2\right)}\\ & c=-B_{12}{\alpha }^2\beta /\lambda \\ & u_0=c{v}_0/\alpha -B_{12}{\alpha }^2/\lambda \end{array}$$
根据上式计算得到A后，可以计算相机外参：
$$\begin{array}{rl}& r_1=k{A}^{-1}{h}_1\\ & r_2=k{A}^{-1}{h}_2\\ & r_3=r_1\times r_2\\ & t=k{A}^{-1}{h}_3\end{array}$$
其中:
$$k=1/\Vert A^{-1}h1\Vert =1/\Vert A^{-1}h2\Vert$$
这里计算得到的R比一定满足标准旋转矩阵的性质，需要优化。

最大似然估计

上述解是通过最小化一个没有物理意义的代数距离而得到的。可以通过最大似然推理来改进它。

若给出模型平面上n张图像，模型平面上有m个点，假设图像点被独立且同分布的噪声破坏。最大似然估计可以通过最小化以下函数来得到：
$$\begin{gathered} \sum _{i=1}^n\sum _{j=1}^m{\Vert m_{ij}-\hat{m}\left(A,R_i,t_i,M_j\right)\Vert }^2 \\ 其中: \\ hatm\left(A,R_i,t_i,M_j\right)为通过公式(1)计算得到的投影点\text{\hspace{1em}(14)} \end{gathered}$$
一个旋转R由一个包含3个参数的向量参数化，用r表示，它平行于旋转轴，其大小等于旋转角。R和r由罗德里格斯公式相关联。最小化公式（14）是一个非线性最小化问题，它可以用在Minpack中实现的LM算法来求解。它需要一个初始参数值：A，{Ri，ti|i=1…n}这个初始参数则可以使用2.2.3.1中所求的参数解。

处理镜头径向畸变

在前面1.5.1中定义了图像的径向畸变系数模型：
$$\begin{array}{c}{x}_{\text{畸变 }}=x\left(1+k_1\ast r^2+k_2\ast r^4+k_3\ast r^6\right)\\ y_{\text{畸变 }}=y\left(1+k_1\ast r^2+k_2\ast r^4+k_3\ast r^6\right)\end{array}$$
在张正友数学模型中只考虑k1,k2：
$$\begin{array}{rl}& \hat{x}=x\left(1+k_1\ast \left(x^2+y^2\right)+k_2\ast {\left(x^2+y^2\right)}^2\right)\\ & \hat{y}=y\left(1+k_1\ast \left(x^2+y^2\right)+k_2\ast {\left(x^2+y^2\right)}^2\right)\end{array}$$

$$其中:\hat{x},\hat{y}为真实观测图像坐标,x,y为理想图像坐标$$

将上式从图像坐标系转到像素坐标系有：
$$\begin{array}{rl}& \hat{u}=u+\left(u-u_0\right)\left(k_1\ast \left(x^2+y^2\right)+k_2\ast {\left(x^2+y^2\right)}^2\right)\\ & \hat{v}=v+\left(v-v_0\right)\left(k_1\ast \left(x^2+y^2\right)+k_2\ast {\left(x^2+y^2\right)}^2\right)\end{array}\text{\hspace{1em}(15)}$$

$$其中:\hat{u},\hat{v}为真实观测像素坐标,u,v为理想像素坐标$$

将上式转换到矩阵形式：
$$\left[\begin{array}{cc}\left(u-u_0\right)\left(x^2+y^2\right)& \left(u-u_0\right){\left(x^2+y^2\right)}^2\\ \left(v-v_0\right)\left(x^2+y^2\right)& \left(v-v_0\right){\left(x^2+y^2\right)}^2\end{array}\right]\left[\begin{array}{l}k1\\ k2\end{array}\right]=\left[\begin{array}{l}\hat{u}-u\\ \hat{v}-v\end{array}\right]$$
因此每个图像上的模型点可以提供上面两个方程，则n张图像m个模型点有2mn个方程，矩阵形式为：
$$\begin{gathered} Dk=d \\ 其中: \\ k=[k1,k2{]}^T \end{gathered}$$
线性最小二乘解可由公式得出：
$$k={\left(D^{T}D\right)}^{-1}{D}^{T}d\text{\hspace{1em}(16)}$$
一旦估计了k1和k2，就可以通过（15）求得的投影点带入公式（14）来细化其他参数的估计，交替使用这两个方法，直到收敛为止。

实验结果表明，上述交替技术的收敛速度较慢。对（14）的一个自然扩展是通过最小化以下函数来估计完整的参数集
$$\begin{gathered} \sum _{i=1}^n\sum _{j=1}^m{\Vert m_{ij}-\hat{m}\left(A,k1,k2,R_i,t_i,M_j\right)\Vert }^2 \\ 其中: \\ \hat{m}\left(A,R_i,t_i,M_j\right)为通过公式(1)对图像i中M_j点投影,然后根据(14)进行变形的点。\text{\hspace{1em}(17)} \end{gathered}$$
上式使用LM算法来求解。需要的初始参数值：A，{Ri，ti|i=1…n}可以使用2.2.3.1中所求的参数解或者2.2.3.2中所求的参数解或者简单设为0.

张正友标定法标定步骤

（1）打印图案（如棋盘格）并将其附着到平面表面上；

（2）通过移动平面或摄像机，拍摄不同方向的图像；

（3）检测图像中的特征点；

（4）利用2.2.3.1所述的封闭解估计五个内在参数和所有外部参数。

（5）通过求解线性最小二乘法（16）来估计径向畸变的系数；

（6）通过最小化（17）来细化所有参数。

参考链接: c++opencv相机标定（calibrateCamera）详解