基于图像的多视角立体视觉三维重建（二）——张正友相机标定原理与代码实现
相机标定概述
相机标定：给定物体的参考坐标 ( x , y , z ) (x,y,z) (x,y,z)和它的像素坐标 ( u , v ) (u,v) (u,v)确定相机的内部参数和外部参数,其中 ( x , y , 0 ) (x,y,0) (x,y,0)表示左边的标定板的坐标, ( u , v ) (u,v) (u,v)表示右边成像平面的坐标。
内参： f x , f y , c x , c y , k 1 , k 2 , k 3 , p 1 , p 2 f_{x}, f_{y}, c_{x}, c_{y}, k_{1}, k_{2}, k_{3}, p_{1}, p_{2} fx,fy,cx,cy,k1,k2,k3,p1,p2
外参： R , t R, t R,t
传统的相机标定方法：
在一定的相机模型下，基于特定的实验条件如形状、尺寸已知的参照物，经过图像处理、数学变换求取相机的内外参数的过程。
自标定法：
不需要依赖参照物、利用相机本身参数之间的约束关系对相机进行标定。通常利用周围图像和图像之间的匹配信息进行标定，自标定法与场景和相机的运动无关。常有基于Krupa方程、分层逐步标定方法、基于二次曲面自标定法。
相机的优化模型可以简化为重投影误差最小化的模型：
∑ i = 1 n ∑ j = 1 m ∣ ∣ m i j − m ^ ( A , R i , t i , M j ) ∣ ∣ 2 (1) \sum_{i=1}^{n}\sum_{j=1}^{m}||m_{ij}-\hat{m}(A,R_{i},t_{i},M_{j})||^{2}\tag{1} i=1∑nj=1∑m∣∣mij−m^(A,Ri,ti,Mj)∣∣2(1)
其中 m i j m_{ij} mij为图像上的特征点的坐标， m ^ ( A , R i , t i , M j ) \hat{m}(A,R_{i},t_{i},M_{j}) m^(A,Ri,ti,Mj)代表三维点经过相机的参数投影到图像上的坐标。优化目标是最小化两者的误差。
思考：为何不直接优化这个模型，得到相机的内外参？
因为要优化的参数过多，模型的解严重依赖初始值，容易陷入局部最优解。
张正友标定法
张正友标定法克服了标定需要高精度的三维标定物的缺点，又解决了自标定法鲁棒性差的难题。标定过程只需要用一个棋盘，并从不同角度拍摄几组图片即可，具有鲁棒性强和精度高的特点 。
原理：
[ u v 1 ] = [ 1 / d X 0 u 0 0 1 / d Y v 0 0 0 1 ] [ f 0 0 0 0 f 0 0 0 0 1 0 ] [ R t 0 1 ] [ x w y w z w 1 ] = [ α x 0 u 0 0 0 α y v 0 0 0 0 1 0 ] [ R t 0 1 ] [ x w y w z w 1 ] = M 1 M 2 X w = M X w (2) \begin{array}{l} {\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]=\left[\begin{array}{ccc} 1 / d X & 0 & u_{0} \\ 0 & 1 / d Y & v_{0} \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{cccc} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}\right]\left[\begin{array}{cc} \mathbf{R} & \mathbf{t} \\ 0 & 1 \end{array}\right]\left[\begin{array}{c} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{array}\right]} \\ =\left[\begin{array}{cccc} \alpha_{x} & 0 & u_{0} & 0 \\ 0 & \alpha_{y} & v_{0} & 0 \\ 0 & 0 & 1 & 0 \end{array}\right]\left[\begin{array}{cc} \mathbf{R} & \mathbf{t} \\ 0 & 1 \end{array}\right]\left[\begin{array}{c} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{array}\right]=\mathbf{M}_{1} \mathbf{M}_{2} \mathbf{X}_{w}=\mathbf{M X}_{w} \end{array}\tag{2} ⎣⎡uv1⎦⎤=⎣⎡1/dX0001/dY0u0v01⎦⎤⎣⎡f000f0001000⎦⎤[R0t1]⎣⎢⎢⎡xwywzw1⎦⎥⎥⎤=⎣⎡αx000αy0u0v01000⎦⎤[R0t1]⎣⎢⎢⎡xwywzw1⎦⎥⎥⎤=M1M2Xw=MXw(2)
假设棋盘位于世界坐标系的 z w = 0 z_{w}=0 zw=0的平面上，即 [ x w y w z w 1 ] \begin{bmatrix} x_{w}\\y_{w}\\z_{w}\\1\end{bmatrix} ⎣⎢⎢⎡xwywzw1⎦⎥⎥⎤中的 z w = 0 z_{w}=0 zw=0得：
[ u v 1 ] = s [ f x γ u 0 0 f y v 0 0 0 1 ] [ r 1 r 2 t ] [ x w y w 1 ] (3) \begin{bmatrix} u\\v\\1\end{bmatrix} = s\begin{bmatrix} f_{x}&\gamma&u_{0}\\0&f_{y}&v_{0}\\0&0&1\end{bmatrix}\begin{bmatrix}r_{1}&r_{2}&t\end{bmatrix}\begin{bmatrix}x_{w}\\y_{w}\\1\end{bmatrix}\tag{3} ⎣⎡uv1⎦⎤=s⎣⎡fx00γfy0u0v01⎦⎤[r1r2t]⎣⎡xwyw1⎦⎤(3)
单应性变换：
当世界坐标系中的点位于同一个平面时，可以用单应性变换来描述物体在世界坐标系和像素坐标系之间的位置映射关系。对应的变换矩阵称为单应性矩阵。即
H = s [ f x γ u 0 0 f y v 0 0 0 1 ] [ r 1 r 2 t ] = s M [ r 1 r 2 t ] (4) H=s\begin{bmatrix}f_{x}&\gamma&u_{0}\\0&f_{y}&v_{0}\\0&0&1\end{bmatrix}\begin{bmatrix}r_{1}&r_{2}&t\end{bmatrix}=sM\begin{bmatrix}r_{1}&r_{2}&t\end{bmatrix}\tag{4} H=s⎣⎡fx00γfy0u0v01⎦⎤[r1r2t]=sM[r1r2t](4)
估计单应性矩阵：
首先，假设图像中特征点的齐次坐标为 ( x ′ , y ′ , 1 ) (x^{'}, y^{'},1) (x′,y′,1),标定板的世界坐标系的齐次坐标简记为 ( x , y , 1 ) (x,y,1) (x,y,1)单应性矩阵H定义为：
H = [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] H=\begin{bmatrix}h_{11}&h_{12}&h_{13}\\h_{21}&h_{22}&h_{23}\\h_{31}&h_{32}&h_{33} \end{bmatrix} H=⎣⎡h11h21h31h12h22h32h13h23h33⎦⎤
其中 [ x ′ y ′ 1 ] = [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] [ x y 1 ] \begin{bmatrix}x^{'}\\y^{'}\\1\end{bmatrix} = \begin{bmatrix}h_{11}&h_{12}&h_{13}\\h_{21}&h_{22}&h_{23}\\h_{31}&h_{32}&h_{33} \end{bmatrix}\begin{bmatrix}x\\y\\1\end{bmatrix} ⎣⎡x′y′1⎦⎤=⎣⎡h11h21h31h12h22h32h13h23h33⎦⎤⎣⎡xy1⎦⎤
得：
x ′ = h 11 x + h 12 y + h 13 h 31 x + h 32 y + h 33 y ′ = h 21 x + h 22 y + h 23 h 31 x + h 32 y + h 33 (5) x^{'}=\frac{h_{11}x+h_{12}y+h_{13}}{h_{31}x+h_{32}y+h_{33}}\\ y^{'}=\frac{h_{21}x+h_{22}y+h_{23}}{h_{31}x+h_{32}y+h_{33}}\tag{5} x′=h31x+h32y+h33h11x+h12y+h13y′=h31x+h32y+h33h21x+h22y+h23(5)
因为采用了齐次坐标系，所以 x ′ , y ′ x^{'},y^{'} x′,y′可以进行任意尺度的缩放,所以H的自由度应该为9 - 1＝8，第一种处理方法是分子分母同除 h 33 h_{33} h33,将 h 33 h_{33} h33固定为1：
x ′ = h 11 x + h 12 y + h 13 h 31 x + h 32 y + 1 y ′ = h 21 x + h 22 y + h 23 h 31 x + h 32 y + 1 x^{'} = \frac{h_{11}x+h_{12}y+h_{13}}{h_{31}x+h_{32}y+1}\\ y^{'}=\frac{h_{21}x+h_{22}y+h_{23}}{h_{31}x+h_{32}y+1} x′=h31x+h32y+1h11x+h12y+h13y′=h31x+h32y+1h21x+h22y+h23
需要9个参数才可以确定H矩阵，第二种方法是人为增加一个约束条件,即：
h 11 2 + h 12 2 + . . . + h 33 2 ＝ 1 h_{11}^{2}+h_{12}^{2}+...+h_{33}^{2} ＝ 1 h112+h122+...+h332＝1
将 x ′ 、 y ′ x^{'}、y^{'} x′、y′的表达式展开得：
[ x 1 y 1 1 0 0 0 − x 1 x 1 ′ − y 1 x 1 ′ − x 1 ′ 0 0 0 x 1 y 1 1 − x 1 y 1 ′ − y 1 y 1 ′ − y 1 ′ ] [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] = [ 0 0 ] (6) \left[\begin{array}{ccccccccc} x_{1} & y_{1} & 1 & 0 & 0 & 0 & -x_{1} x_{1}^{\prime} & -y_{1} x_{1}^{\prime} & -x_{1}^{\prime} \\ 0 & 0 & 0 & x_{1} & y_{1} & 1 & -x_{1} y_{1}^{\prime} & -y_{1} y_{1}^{\prime} & -y_{1}^{\prime} \\ \end{array}\right]\left[\begin{array}{l} h_{11} \\ h_{12} \\ h_{13} \\ h_{21} \\ h_{22} \\ h_{23} \\ h_{31} \\ h_{32} \\ h_{33} \end{array}\right]=\left[\begin{array}{l} 0 \\ 0 \\ \end{array}\right]\tag{6} [x10y10100x10y101−x1x1′−x1y1′−y1x1′−y1y1′−x1′−y1′]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡h11h12h13h21h22h23h31h32h33⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=[00](6)
由上式可以得到，一对点可以提供两个约束，而H的自由度为8，至少需要4对点才可以求解这个方程。
实际上，特征点存在一定的噪声，且存在一些错误的匹配点，如果只用4对点来计算单应矩阵会存在较大的误差。因此需要用多组点使用Ransac算法来计算单应矩阵。
由单应矩阵求解相机内参：
先不考虑镜头的畸变，将单应矩阵H记为 H = [ h 1 , h 2 , h 3 ] H=[h_{1}, h_{2}, h_{3}] H=[h1,h2,h3] 得:
h 1 = s M r 1 或 r 1 = λ M − 1 h 1 h 2 = s M r 2 或 r 2 = λ M − 1 h 2 h 3 = s M t 或 t = λ M − 1 h 3 h_{1}=sMr_{1} 或 r_{1}=\lambda M^{-1}h_{1} \\ h_{2}=sMr_{2} 或 r_{2}=\lambda M^{-1}h_{2}\\h_{3}=sMt 或 t=\lambda M^{-1}h_{3} h1=sMr1或r1=λM−1h1h2=sMr2或r2=λM−1h2h3=sMt或t=λM−1h3
其中 λ = s − 1 \lambda = s^{-1} λ=s−1，已知旋转向量 r 1 、 r 2 r_{1}、r_{2} r1、r2相互正交，即： r 1 T r 2 = 0 r_{1}^{T}r_{2}=0 r1Tr2=0,将上式代入得： h 1 T ( M − 1 ) T M − 1 h 2 = 0 h_{1}^{T}(M^{-1})^{T}M^{-1}h_{2}=0 h1T(M−1)TM−1h2=0
且旋转向量模长相等，即 ∣ ∣ r 1 ∣ ∣ = ∣ ∣ r 2 ∣ ∣ = = 1 ||r_{1}||=||r_{2}||==1 ∣∣r1∣∣=∣∣r2∣∣==1得：
h 1 T ( M − 1 ) T M − 1 h 1 = h 2 T ( M − 1 ) T M − 1 h 2 (7) h_{1}^{T}(M^{-1})^{T}M^{-1}h_{1}=h_{2}^{T}(M^{-1})^{T}M^{-1}h_{2}\tag{7} h1T(M−1)TM−1h1=h2T(M−1)TM−1h2(7)
一个单应矩阵可以提供以上两个约束条件，使用两个约束条件求取内参与外参，已知 M = [ f x γ u 0 0 f y v 0 0 0 1 ] M = \begin{bmatrix}f_{x}&\gamma&u_{0}\\ 0 &f_{y} &v_{0} \\ 0 &0 &1 \end{bmatrix} M=⎣⎡fx00γfy0u0v01⎦⎤，记 B = ( M − 1 ) T M − 1 B=(M^{-1})^{T}M^{-1} B=(M−1)TM−1得:
B = ( M − 1 ) r M − 1 = [ 1 f x 2 − γ f x 2 f y v 0 γ − u 0 f y f x 2 f y − γ f x 2 f y γ 2 f x 2 f y 2 + 1 f y 2 − γ v 0 γ − u 0 f y f x 2 f y 2 − v 0 f y 2 v 0 γ − u 0 f y f x 2 f y − γ v 0 γ − u 0 f y f x 2 f y 2 − v 0 f y 2 ( v 0 γ − u 0 f y ) 2 f x 2 f y 2 + v 0 2 f y 2 + 1 ] = [ B 11 B 12 B 13 B 21 B 22 B 23 B 31 B 32 B 33 ] (8) B=\left(M^{-1}\right)^{r} M^{-1}=\left[\begin{array}{ccc} \frac{1}{f_{x}^{2}} & -\frac{\gamma}{f_{x}^{2} f_{y}} & \frac{v_{0} \gamma-u_{0} f_{y}}{f_{x}^{2} f_{y}} \\\;\\ -\frac{\gamma}{f_{x}^{2} f_{y}} & \frac{\gamma^{2}}{f_{x}^{2} f_{y}^{2}}+\frac{1}{f_{y}^{2}} & -\gamma \frac{v_{0} \gamma-u_{0} f_{y}}{f_{x}^{2} f_{y}^{2}}-\frac{v_{0}}{f_{y}^{2}} \\\;\\ \frac{v_{0} \gamma-u_{0} f_{y}}{f_{x}^{2} f_{y}} & -\gamma \frac{v_{0} \gamma-u_{0} f_{y}}{f_{x}^{2} f_{y}^{2}}-\frac{v_{0}}{f_{y}^{2}} & \frac{\left(v_{0} \gamma-u_{0} f_{y}\right)^{2}}{f_{x}^{2} f_{y}^{2}}+\frac{v_{0}^{2}}{f_{y}^{2}}+1 \end{array}\right]=\left[\begin{array}{ccc} B_{11} & B_{12} & B_{13} \\ B_{21} & B_{22} & B_{23} \\ B_{31} & B_{32} & B_{33} \end{array}\right]\tag{8} B=(M−1)rM−1=⎣⎢⎢⎢⎢⎢⎢⎡fx21−fx2fyγfx2fyv0γ−u0fy−fx2fyγfx2fy2γ2+fy21−γfx2fy2v0γ−u0fy−fy2v0fx2fyv0γ−u0fy−γfx2fy2v0γ−u0fy−fy2v0fx2fy2(v0γ−u0fy)2+fy2v02+1⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎡B11B21B31B12B22B32B13B23B33⎦⎤(8)
其中B为对称矩阵.
定义 H = [ h 1 , h 2 , h 3 ] H=[h_{1}, h_{2}, h_{3}] H=[h1,h2,h3]中的 h i = [ h i 1 , h i 2 , h i 3 ] T h_{i}=[h_{i1}, h_{i2},h_{i3}]^{T} hi=[hi1,hi2,hi3]T由 h 1 B h 2 = 0 和 h 1 T B h 1 = h 2 T B h 2 h_{1}Bh_{2}=0和h_{1}^{T}Bh_{1}=h_{2}^{T}Bh_{2} h1Bh2=0和h1TBh1=h2TBh2两个约束条件得:
h t T B h j = [ h i 1 h j 1 h i 1 h j 2 + h i 2 h j 1 h 22 h j 2 h i 3 h j 1 + h i 1 h j 3 h i 3 h j 2 + h i 2 h j 3 h i 3 h j 3 ] T [ B 11 B 12 B 22 B 13 B 23 B 33 ] (9) h_{t}^{T} B h_{j}=\left[\begin{array}{c} h_{\mathrm{i1}} h_{j 1} \\ h_{i 1} h_{j 2}+h_{i 2} h_{j 1} \\ h_{22} h_{j 2} \\ h_{i 3} h_{j 1}+h_{\mathrm{i} 1} h_{j 3} \\ h_{i3} h_{j 2}+h_{i 2} h_{j 3} \\ h_{i3} h_{j 3} \end{array}\right]^{T}\left[\begin{array}{c} B_{11} \\ B_{12} \\ B_{22} \\ B_{13} \\ B_{23} \\ B_{33} \end{array}\right]\tag{9} htTBhj=⎣⎢⎢⎢⎢⎢⎢⎡hi1hj1hi1hj2+hi2hj1h22hj2hi3hj1+hi1hj3hi3hj2+hi2hj3hi3hj3⎦⎥⎥⎥⎥⎥⎥⎤T⎣⎢⎢⎢⎢⎢⎢⎡B11B12B22B13B23B33⎦⎥⎥⎥⎥⎥⎥⎤(9)
为了简化表达形式，令
v i j = [ h i 1 h j 1 h i 1 h j 2 + h i 2 h j 1 h i 2 h j 2 h i 3 h j 1 + h i 1 h j 3 h i 3 h j 2 + h i 2 h j 3 h i 3 h j 3 ] , b = [ B 11 B 12 B 22 B 13 B 23 B 33 ] v_{i j}=\left[\begin{array}{c} h_{\mathrm{i1}} h_{j 1} \\ h_{i 1} h_{j 2}+h_{i 2} h_{j 1} \\ h_{i 2} h_{j 2} \\ h_{i 3} h_{j 1}+h_{i 1} h_{j 3} \\ h_{i 3} h_{j 2}+h_{i 2} h_{j 3} \\ h_{i 3} h_{j 3} \end{array}\right], \quad b=\left[\begin{array}{c} B_{11} \\ B_{12} \\ B_{22} \\ B_{13} \\ B_{23} \\ B_{33} \end{array}\right] vij=⎣⎢⎢⎢⎢⎢⎢⎡hi1hj1hi1hj2+hi2hj1hi2hj2hi3hj1+hi1hj3hi3hj2+hi2hj3hi3hj3⎦⎥⎥⎥⎥⎥⎥⎤,b=⎣⎢⎢⎢⎢⎢⎢⎡B11B12B22B13B23B33⎦⎥⎥⎥⎥⎥⎥⎤
则 v 12 b = 0 ( v 11 T − v 22 T ) b = 0 v_{12}b=0\\(v_{11}^{T} - v_{22}^{T})b=0 v12b=0(v11T−v22T)b=0
写成矩阵形式得：
[ v 12 T v 11 T − v 22 T ] b = 0 (10) \begin{bmatrix}v_{12}^{T}\\v_{11}^{T} - v_{22}^{T}\end{bmatrix}b=0\tag{10} [v12Tv11T−v22T]b=0(10)
如果拍摄了n张不同角度的标定图片，每张图片可以通过四点法来计算得到H矩阵，由H矩阵的两个约束可以得到以上两个B矩阵的约束，由于B矩阵的自由度为6（B为对称矩阵），至少需要3张以上的图片，才能解得B矩阵。最后根据B的表达式反解除相机的内参：
由式8得：
得: { f x = λ / B 11 f y = λ B 11 / ( B 11 B 22 − B 12 2 ) u 0 = γ v 0 / f y − B 13 f x 2 / λ v 0 = ( B 12 B 13 − B 11 B 23 ) / ( B 11 B 22 − B 12 2 ) γ = − B 12 f x 2 f y / λ λ = B 33 − [ B 13 2 + v 0 ( B 12 B 13 − B 11 B 23 ) ] / B 11 (11) \left\{\begin{aligned} f_{x} &=\sqrt{\lambda / B_{11}} \\ f_{y} &=\sqrt{\lambda B_{11} /\left(B_{11} B_{22}-B_{12}^{2}\right)} \\ u_{0} &=\gamma v_{0} / f_{y}-B_{13} f_{x}^{2} / \lambda \\ v_{0} &=\left(B_{12} B_{13}-B_{11} B_{23}\right) /\left(B_{11} B_{22}-B_{12}^{2}\right) \\ \gamma &=-B_{12} f_{x}^{2} f_{y} / \lambda \\ \lambda &=B_{33}-\left[B_{13}^{2}+v_{0}\left(B_{12} B_{13}-B_{11} B_{23}\right)\right] / B_{11} \end{aligned}\right.\tag{11} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧fxfyu0v0γλ=λ/B11=λB11/(B11B22−B122)=γv0/fy−B13fx2/λ=(B12B13−B11B23)/(B11B22−B122)=−B12fx2fy/λ=B33−[B132+v0(B12B13−B11B23)]/B11(11)
由单应矩阵求解相机外参
得到内参矩阵M与单应矩阵H求解外参矩阵 [ r 1 , r 2 , t ] [r_{1}, r_{2}, t] [r1,r2,t]即：
H = [ h 1 , h 2 , h 3 ] = s [ f x γ u 0 0 f y v 0 0 0 1 ] [ r 1 , r 2 , t ] = s M [ r 1 , r 2 , t ] (12) H=[h_{1}, h_{2}, h_{3}]=s\begin{bmatrix}f_{x}&\gamma &u_{0}\\0&f_{y}&v_{0}\\0&0&1\end{bmatrix}[r_{1}, r_{2}, t]=sM[r_{1}, r_{2}, t]\tag{12} H=[h1,h2,h3]=s⎣⎡fx00γfy0u0v01⎦⎤[r1,r2,t]=sM[r1,r2,t](12)
其中：
r 1 = λ M − 1 h 1 r 2 = λ M − 1 h 2 r 3 = r 1 × r 2 t = λ M − 1 h 3 ∣ ∣ r 1 ∣ ∣ = ∣ ∣ λ M − 1 h 1 ∣ ∣ = 1 λ = 1 / ∣ ∣ M − 1 h 1 ∣ ∣ r_{1}=\lambda M^{-1}h_{1}\\r_{2}=\lambda M^{-1}h_{2}\\r_{3}=r_{1}\times r_{2}\\t=\lambda M^{-1}h_{3}\\||r_{1}||=||\lambda M^{-1}h_{1}||=1\\\lambda=1/||M^{-1}h_{1}|| r1=λM−1h1r2=λM−1h2r3=r1×r2t=λM−1h3∣∣r1∣∣=∣∣λM−1h1∣∣=1λ=1/∣∣M−1h1∣∣
优化参数
计算得到相机内外参数的初始值后，需要基于重投影误差最小化捆绑调整继续优化相机参数。假设噪声是独立同分布的，优化的目标函数如下：
∑ i = 1 n ∑ j = 1 m ∥ x i j − x ′ ( M , R i , t i , X j ) ∥ 2 (13) \sum_{i=1}^{n} \sum_{j=1}^{m}\left\|x_{i j}-x^{\prime}\left(M, R_{i}, t_{i}, X_{j}\right)\right\|^{2}\tag{13} i=1∑nj=1∑m∥xij−x′(M,Ri,ti,Xj)∥2(13)
其中 x i j x_{ij} xij为标定板上第j个点的对应的特征点在像平面上位置坐标，M为相机内参， R i 、 t i R_{i}、t_{i} Ri、ti为第i个相机的旋转矩阵与平移矩阵， X j X_{j} Xj为三维空间中标定板上第j个特征点的坐标。
由于相机成像过程中受到径向畸变的映像，因此将径向畸变系数 k 1 , k 2 k_{1}, k_{2} k1,k2一起进行捆绑调整，一般 k 1 , k 2 k_{1}, k_{2} k1,k2初始值设置为0，常用的优化算法为LM算法。
∑ i = 1 n ∑ j = 1 m ∥ x i j − x ′ ( M , k 1 , k 2 , R i , t i , X j ) ∥ 2 (14) \sum_{i=1}^{n} \sum_{j=1}^{m}\left\|x_{i j}-x^{\prime}\left(M, k_{1}, k_{2}, R_{i}, t_{i}, X_{j}\right)\right\|^{2}\tag{14} i=1∑nj=1∑m∥xij−x′(M,k1,k2,Ri,ti,Xj)∥2(14)
棋盘格标定板与圆形标定板选择
圆形检测精度高、表现为中心拟合精度高，但是圆会出现偏心误差, 棋盘格角点检测精度较低但不存在偏心误差，因为直线的投影依然是直线，角点为投影后直线的交点。
单目相机标定代码实现（基于python：opencv）
import os
import cv2
import numpy as np
import glob
import matplotlib.pylab as plt
# 单目标定
class Calibrator:
def __init__(self):
self.img_size = None # 图像尺寸（H, W）
self.points_world_xyz = [] # 世界坐标
self.points_pixel_xy = [] # 像素坐标
self.error = None # 重投影误差
self.mtx = None # 内参矩阵：k1、k2、p1、p2、k3
self.dist = None # 畸变系数
self.rvecs = None # 旋转矩阵
self.tvecs = None # 平移矩阵
self.calib_info = {}
def detect(self, cols, rows, folder, show):
assert ((cols > 0) & (rows > 0))
# 相机标定评价标准与结束条件
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# 角点的世界坐标，几个像素点对应几个三维点，增加了一个深度信息
point_world_xyz = np.zeros((rows * cols, 3), np.float32)
point_world_xyz[:, :2] = np.mgrid[0: cols, 0: rows].T.reshape(-1, 2)
# 用于标定的图片
calib_files = sorted(glob.glob(os.path.join(folder, '*jpg')))
for filename in calib_files:
img = cv2.imread(filename)
if img is None:
raise FileNotFoundError(filename, "没有找到图片!")
# 灰度化
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
if self.img_size is None:
self.img_size = gray.shape[::-1]
else:
assert gray.shape[::-1] == self.img_size
# 01 角点粗检测
ret, corners = cv2.findChessboardCorners(gray, (cols, rows), None)
if ret:
# 02 角点精检测
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
# 03 添加角点像素坐标、世界坐标
self.points_pixel_xy.append(corners)
self.points_world_xyz.append(point_world_xyz)
else:
print("未检测到角点:", filename)
if show:
if len(img.shape) == 2:
print(img.shape)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img = cv2.drawChessboardCorners(img, (cols, rows), corners, ret)
title = os.path.basename(filename)
cv2.imshow(title, img)
cv2.moveWindow(title, 500, 200)
cv2.waitKey(0)
def calib(self):
# 03 标定
self.error, self.mtx, self.dist, self.rvecs, self.tvecs = cv2.calibrateCamera(
self.points_world_xyz, # 世界坐标
self.points_pixel_xy, # 像素坐标
self.img_size, # 图像尺寸
None, None
)
def rectify(self, img):
# 04 使用 mtx： 相机内参矩阵 dist: 畸变系数矩阵
return cv2.undistort(img, self.mtx, self.dist)
def save_calib_info(self, save_file):
# 效果好坏评价
print("重投影误差:\n", self.error, "\n")
print("相机内参:\n", self.mtx, "\n")
print("相机畸变:\n", self.dist, "\n")
# 每个标定板（左上角角点）相对于相机原点的平移、旋转矩阵
print("旋转矩阵:\n", self.rvecs, "\n")
print("平移矩阵:\n", self.tvecs, "\n")
self.calib_info["Error"] = self.error
self.calib_info["Ac"] = self.mtx
self.calib_info["Dist"] = self.dist
self.calib_info["Rs"] = self.rvecs
self.calib_info["Ts"] = self.tvecs
np.save(save_file, self.calib_info)
print("保存标定信息到文件:", save_file)
@staticmethod
def imread(filename: str):
return cv2.imdecode(np.fromfile(filename, dtype=np.uint8), -1)
if __name__ == '__main__':
# 标定图像路径
folder = r"./imgs"
# 标定参数保存
save_file = r"./calib_info.npy"
show = False
cols = 9 # 有多少列角点
rows = 6 # 有多少行角点
# 初始化
calibrator = Calibrator()
# 01 检测角点
calibrator.detect(cols, rows, folder, show)
# 02 标定相机
calibrator.calib()
# 03 保存标定
calibrator.save_calib_info(save_file)
# 04 校正图片
img_test = calibrator.imread(os.path.join(folder, "1.jpg"))
dst1 = calibrator.rectify(img_test)