作者：Frank
时间：2016-07-04

---

所谓相机标定是只对相机通过特征提取等手段获取相机的内参和畸变。其中内参包括焦距fx,fy (分别为x方向和y方向的尺度因子)和光轴中心cx,xy。

1. 基础知识

1.1. 三维旋转

1.1.1. 基元旋转

所谓基元旋转是指在oxyz坐标系中，坐标系围绕某坐标轴旋转后得到的结果。
在oxyz坐标系中，坐标系围绕x轴旋转的示意图如下：

则通过示意图得到的对应的转换矩阵为:
$${\rm{T}}(\phi ) = \left[ {\matrix{
1 & 0 & 0 \cr
0 & {\cos \phi } & {\sin \phi } \cr
0 & { - \sin \phi } & {\cos \phi } \cr
} } \right] \tag{1} $$
同理，oxyz绕oy轴和oz轴旋转对应的转换矩阵分别为：
$${\rm{T}}(\theta ) = \left[ {\matrix{
{\cos \theta } & 0 & { - \sin \theta } \cr
0 & 1 & 0 \cr
{\sin \theta } & 0 & {\cos \theta } \cr
} } \right] \tag{2}$$
和
$${\rm{T}}(\varphi ) = \left[ {\matrix{
{\cos \varphi } & {\sin \varphi } & 0 \cr
{ - \sin \varphi } & {\cos \varphi } & 0 \cr
0 & 0 & 1 \cr
} } \right] \tag{3}$$

1.1.2. 坐标变换矩阵

对于任意两个oxyz坐标系而言，其相对的转换矩阵有两种情况：

1. 假如两个坐标系共原点。若存在一个空间点处于两个坐标系中，则该空间点在两个坐标系间表示的转换只存在一个旋转矩阵。设该旋转矩阵为$R$，则可以将R分解为依次沿ox轴，oy轴，oz轴旋转的基元旋转矩阵的左乘结果。即：
   $$R ={R_{oz}} \cdot {R_{oy}} \cdot {R_{ox}}=\left[ {\matrix{
   {\cos \varphi } & {\sin \varphi } & 0 \cr
   { - \sin \varphi } & {\cos \varphi } & 0 \cr
   0 & 0 & 1 \cr
   } } \right] \cdot \left[ {\matrix{
   {\cos \theta } & 0 & { - \sin \theta } \cr
   0 & 1 & 0 \cr
   {\sin \theta } & 0 & {\cos \theta } \cr
   } } \right] \cdot \left[ {\matrix{
   1 & 0 & 0 \cr
   0 & {\cos \phi } & {\sin \phi } \cr
   0 & { - \sin \phi } & {\cos \phi } \cr
   } } \right] \tag{4}$$
2. 假如两个坐标系不共原点，则空间点的转换矩阵在上述的旋转矩阵后还存在一个原点的相对位移。假设两个坐标系原点之间的相对平移矢量为：${\left[ {\matrix{
   {X_T} & {Y_T} & {Z_T} \cr
   } } \right]^T}$ ，则有：
   $$\left[ {\matrix{
   {X_2} \cr
   {Y_2} \cr
   {Z_2} \cr
   } } \right] = R \cdot \left[ {\matrix{
   {X_1} \cr
   {Y_1} \cr
   {Z_1} \cr
   } } \right] + \left[ {\matrix{
   {X_T} \cr
   {Y_T} \cr
   {Z_T} \cr
   } } \right] \tag{5} $$
   将该式改写为其次坐标系，有：
   $$\left[ {\matrix{
   {X_2} \cr
   {Y_2} \cr
   {Z_2} \cr
   1 \cr
   } } \right] = \left[ {\matrix{
   {R_{3 \times 3}} & {T_{3 \times 1}} \cr
   O & 1 \cr
   } } \right] \cdot \left[ {\matrix{
   {X_1} \cr
   {Y_1} \cr
   {Z_1} \cr
   1 \cr
   } } \right] \tag{6}$$

1.2. SLAM中的四个坐标系

在SLAM系统中存在四个坐标系，即：摄像机坐标系 、 图像物理坐标系 、 像素坐标系 和 世界坐标系（参考坐标系）。而SLAM的运算涉及到点坐标在上述坐标系之间的转换。

1.2.1. 图像物理坐标系(x,y) 到 像素坐标系(u,v)

对于图像物理坐标系和像素坐标系，可能存在以下两种情况：

1. 图像坐标系和像素坐标系都是直角坐标系
   
   图像上的每点坐标(u,v) 分别表示在每一帧采集的图像在系统中存储的数组的列数和行数，坐标(u,v)所对应的值就是当前点的灰度值，所以坐标系$uov$又称为像素坐标系。
   同时，为了建立图像中各点的像素与实际的物理尺寸的联系，我们还要建立图像物理 坐标系$x{o_1}y$ 。设点${o_1}$ 在像素坐标系中的坐标为$({u_0},{v_0})$，每个像素沿x轴的实际物理尺寸大小是dx，沿y轴的实际物理尺寸大小为dy,则能得到两个坐标系之间的关系式。

2. 两坐标轴只有一个轴平行，一个轴不平行
   
   ${o_1}$ 在u,v中的坐标为$(u_0,v_0)$，像素在轴上的物理尺寸为dx,dy，则二者之间的仿射变换为：
   $$u = u_0 + {x_d \over dx} - {y_d \cot \theta \over dx} \tag{7}$$
   $$v = v_0 + {y_d \over {dy \sin \theta}} \tag{8}$$
   表示成齐次坐标的形式为：
   $$\left[ {\matrix{
   u \cr
   v \cr
   1 \cr
   } } \right] = \left[ {\matrix{
   f_u & {- f_u \cot \theta} & u_0 \cr
   0 & {f_v \over {\sin \theta}} & v_0 \cr
   0 & 0 & 1 \cr
   } } \right] \cdot \left[ {\matrix{
   x_d \cr
   y_d \cr
   1 \cr
   } } \right] \tag{9}$$
   其中 $f_u={1 \over dx}$， $f_v={1 \over dy}$
   为了方便公式的推导，将上式表示为：
   $$\left[ {\matrix{
   u \cr
   v \cr
   1 \cr
   } } \right] = \left[ {\matrix{
   {1 \over S_x} & r & u_0 \cr
   0 & {1 \over S_y} & v_0 \cr
   0 & 0 & 1 \cr
   } } \right] \cdot \left[ {\matrix{
   x \cr
   y \cr
   1 \cr
   } } \right] \tag{10}$$

1.2.2. 图像物理坐标系(x,y)到摄像机坐标系(Xc,Yc,Zc)

示意图如下所示:

利用三角形相似可得：
$${Z_c} \cdot \left[ {\matrix{
x \cr
y \cr
1 \cr
} } \right] = \left[ {\matrix{
{\matrix{
f & 0 & 0 & 0 \cr
} } \cr
{\matrix{
0 & f & 0 & 0 \cr
} } \cr
{\matrix{
0 & 0 & 1 & 0 \cr
} } \cr
} } \right] \cdot \left[ {\matrix{
X_c \cr
Y_c \cr
Z_c \cr
1 \cr
} } \right] \tag{11}$$

1.2.3. 摄像机坐标系(Xc,Yc,Zc) 到世界坐标系(Xw,Yw,Yw)

示意图如下所示：

转换关系为：
$$\left[ {\matrix{
X_c \cr
Y_c \cr
Z_c \cr
1 \cr
} } \right] = \left[ {\matrix{
R_{3 \times 3} & T_{3 \times 1} \cr
} } \right] \cdot \left[ {\matrix{
X_w \cr
Y_w \cr
Z_w \cr
1 \cr
} } \right] \tag{12}$$

1.2.4. 合并公式 + 总结

对上述公式进行总结整理可得：
$${1 \over Z_c}\left[ {\matrix{
u \cr
v \cr
1 \cr
} } \right] = \left[ {\matrix{
{f \over S_x} & r & u_0 \cr
0 & {f \over S_y} & v_0 \cr
0 & 0 & 1 \cr
} } \right] \cdot \left[ {\matrix{
R_{3 \times 3} & T_{3 \times 1} \cr
} } \right] \cdot \left[ {\matrix{
X_w \cr
Y_w \cr
Z_w \cr
1 \cr
} } \right] = K_{3 \times 3} \cdot \left[ {\matrix{
R_{3 \times 3} & T_{3 \times 1} \cr
} } \right] \cdot \left[ {\matrix{
X_w \cr
Y_w \cr
Z_w \cr
1 \cr
} } \right] \tag{13}$$
$${\rm{s}}\left[ {\matrix{
u \cr
v \cr
1 \cr
} } \right] = {P_{3 \times 4}} \cdot \left[ {\matrix{
X_w \cr
Y_w \cr
Z_w \cr
1 \cr
} } \right] \tag{14}$$

申明：本部分借鉴转载来源：翔的专栏-摄像机标定(1) 标定中的四个坐标系

2. 张正友标定法

paper来源：A Flexible New Technique for Camera Calibration.

2.1. 基本方程

对于节1.2.4中的综合公式，我们假设一个空间点的图像坐标表示为$m={[u,v]}^{T}$，空间坐标表示为$M={[X,Y,Z]}^{T}$，其对应的齐次方程表示分别为：$\rm{\tilde m} = [u,v,1]^T$ 和$\rm{\tilde M} = [X,Y,Z,1]^T$，则式(14)可以表示为：
$$\rm{s\tilde m} = A[R,t]\tilde M \tag{15}$$
其中s表示任意数尺度，A表示内参矩阵，$[R,t]$表示旋转和平移。
将$R$分解为列向量有：$R=[r_1,r_2,r_3]$。
因为张正友标定法是基于棋盘格的标定法，为了求得相机的内参，我们可以从棋盘格的平面性入手求得棋盘格从一个平面到另一个平面的投影映射。不妨设$Z=0$，则有：
$${\rm{s}}\left[ {\matrix{
u \cr
v \cr
1 \cr
} } \right] = A \cdot \left[ {\matrix{
r_1 & r_2 & r_3 & t \cr
} } \right] \cdot \left[ {\matrix{
X \cr
Y \cr
0 \cr
1 \cr
} } \right]= A \cdot \left[ {\matrix{
r_1 & r_2 & t \cr
} } \right] \cdot \left[ {\matrix{
X \cr
Y \cr
1 \cr
} } \right] \tag{16}$$
令$H=A \left[ {\matrix{ r_1 & r_2 & t \cr } } \right] $，即：
$${\rm{s}}\left[ {\matrix{
u \cr
v \cr
1 \cr
} } \right] =H \cdot \left[ {\matrix{
X \cr
Y \cr
1 \cr
} } \right] \tag{17}$$
同时将H也分解为列向量：$H=\left[ {\matrix{ h_1 & h_2 & h_3 \cr } } \right] $，因为$H$是一个3x3的矩阵，并且有一个元素作为齐次坐标，因此，H有8个未知量待解(其中A里面有5个未知量，而后面的旋转位移有三个未知量，因此一共是8个未知量)。而$(X,Y)$是标定物的坐标，在棋盘格中可以通过人为控制，因此是已知量。$(u,v)$是像素坐标，直接通过摄像机获得。
由$H= \left[ {\matrix{h_1&h_2&h_3 \cr}} \right] =\lambda \cdot A \left[ { \matrix{r_1&r_2&t \cr}} \right] $可得：
$$r_1={1 \over \lambda}{A^{-1}} h_1 ,r_2={1 \over \lambda}{A^{-1}} h_2 \tag{18} $$
因为$r_1,r_2$是单位正交矢量，因此有：${r_1}^2={r_2}^2=1 , r_1 \times r_2=0 , {r_1}^T \times {r_2}^T =0 $，将式{18}代入可得约束条件：
$${h_1}^T A^{-T} A^{-1} h_2=0 , {h_1}^T A^{-T} A^{-1} h_1={h_2}^T A^{-T} A^{-1} h_2 \tag{19} $$
这就是用来求单应举证$H$的两个基本约束方程。上式中的$h_1,h_2$是通过求解单应性矩阵 $H$得到的。而A中含有5个参数，如果需要完全解出来这5个未知量，我们需要3个不用的单应性矩阵$H$(3个单应性矩阵在2个约束条件下可以产生6个方程)。

2.2. 单应性矩阵$H$的求解

由$H=A \left[ { \matrix{ r_1 & r_2 & t \cr}}\right]=\left[ {\matrix{h_1 & h_2 &h_3 \cr}} \right]=\left[{ \matrix{
h_{11} & h_{12} & h_{13} \cr
h_{21} & h_{22} & h_{23} \cr
h_{31} & h_{32} & 1 \cr
}}\right] , \rm{s\tilde m} = H \tilde M $可得：
$$\left\{ {\matrix{
su = h_{11}X + h_{12}Y + h_{13} \cr
su = h_{21}X + h_{22}Y + h_{23 } \cr
s = h_{31}X + h_{32} + 1 \cr
} } \right. \tag{20}$$
整理可得：
$$\left\{ {\matrix{
uX{h_{31}}+uY{h_{32}}+u={h_{11}}X+{h_{12}}Y+h_{13} \cr
vX{h_{31}}+vY{h_{32}}+v={h_{21}}X+{h_{22}}Y+h_{23} \cr
} } \right. \tag{21}$$
不妨设$h’=\left[{\matrix{h_{11} &h_{12} &h_{13} &h_{21} &h_{22} &h_{23} &h_{31} &h_{32} &1}}\right] $，则上式可表示为：
$$\left[{\matrix{
X&Y&1&0&0&0&-uX&-uY&-u \cr
0&0&0&X&Y&1&-vX&-vY&-v \cr
}}\right]h’=0 \tag{22}$$
上式可以看成是$Sh’=0$，那么矩阵${S^T}S$最小特征值对应的特征向量就是该方程的最小二乘解。再将解归一化得到所需的$h’$，从而可以求得$H$。由于线性解法所得到的解一般不是最优的解，所以可以选取上面两个等式中的一个，构建评价函数，利用Levenberg-Marquarat算法计算出更高精度的解。
该部分参考来源：
[1] 张正友相机标定法
[2] 张氏标定法

2.3. 摄像机内外参数的求解

令$$B={A^{-T}}{A^{-1}}=\left[{ \matrix{
B_{11}&B_{12}&B_{13} \cr
B_{21}&B_{22}&B_{23} \cr
B_{31}&B_{32}&B_{33} \cr
}}\right] =\left[ {\matrix{
1 \over {\alpha ^2} & - \gamma \over{ {\alpha ^2}\beta } & {v_0} \gamma - {u_0} \beta \over {\alpha ^2} \beta \cr
- \gamma \over {\alpha ^2}\beta & {\gamma ^2 \over {\alpha ^2}{\beta ^2}} + {1 \over \beta ^2 } & { - \gamma ({v_0}\gamma - {u_0}\beta ) \over {\alpha ^2}{\beta ^2}} - {v_0 \over \beta ^2} \cr
{v_0} \gamma - {u_0} \beta \over {\alpha ^2} \beta & { - \gamma ({v_0}\gamma - {u_0}\beta ) \over {\alpha ^2}{\beta ^2}} - {v_0 \over \beta ^2} & {({v_0}\gamma - {u_0}\beta )^2 \over {\alpha ^2}{\beta ^2}} + {v_0^2 \over \beta ^2} + 1 \cr
} } \right] \tag{23}$$
由于$B$是对称的，所以$B$可以用一个6D的矢量来表示：
$$b=\left[{\matrix{B_{11}&B_{12}&B_{22}&B_{13}&B_{23}&B_{33} \cr}}\right]^T \tag{24} $$
假设单应矩阵$H$第i列矢量为：${h_i}=\left[{\matrix{h_{i1}&h_{i2}&h_{i3} \cr}}\right]^T $，那么，我们可以得到：
$$ {h_i^T}B{h_j}={v_{ij}^T}b \tag{25} $$
其中：$$v_{ij}=\left[{\matrix{
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} \cr
}}\right]^T \tag{26} $$利用式(19)的约束条件可以得到两个关于b的单应方程：
$$\left[{\matrix{
v_{12}^T \cr
(v_{11}-v_{22})^T \cr
}}\right]b=0 \tag{27}$$
假设有N幅模板图像，就可以得到：
$$Vb=0 \tag{28} $$
其中，V是一个$2N \times 6$ 的矩阵，如果$N \ge 3$，b就可以被解出，从而可以得到5个内参：
$$\left\{ {\matrix{
{v_0} =(B_{12}B_{13}-B_{11}B_{23}) / (B_{11}B_{22}-B_{12}^2) \cr
\lambda =B_{33}-[B_{13}^2+{v-0}(B_{12}B_{13}-B_{11}B_{23})]/B_{11} \cr
\alpha =\sqrt{\lambda / B_{11}} \cr
\beta = \sqrt{\lambda B_{11}/(B_{12}B_{22}-B_{12}^2)} \cr
\gamma =-B_12 \alpha^2\beta/\lambda \cr
{u_0}=\gamma v_0/\alpha -B_13 \alpha^2 /\lambda \cr
}}\right. \tag{29} $$
当求出了内参A时，我们就可以得到对应的旋转和位移矢量：

$r_1=\lambda A^{-1} h_1,r_2=\lambda A^{-1} h_2, r_3=r_1 \times r_2, t=\lambda A^{-1}h_3 $

2.4. 最大似然估计

节2.3.得到的旋转矩阵$R$的方法是基于最小距离的，而在实际中$R=(r_1,r_2,r_3)$并不满足正交性质，所以可以利用最大似然估计来求解。
我们通过移动棋盘格和相机的相对位置来得到关于棋盘格的n幅图像，假设在每一幅图像中都具有数量相同的标定点，且其个数为m，并假设背个表定点的坐标都有独立同分布的噪声，我们可以构造最大似然估计的能量函数：
$$\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^m {||m_{ij} - \hat m(A,R_i,t_i,M_j)||^2} } \tag{30}$$
其中$m_{ij}$是第j个标定点在第i幅图像上的像素坐标矢量，$R_i$是第i幅图像的旋转矩阵,$t_i$是对应的平移向量，$M_j$是第j个标定点的空间坐标,$\hat m(A,R_i,t_i,M_j)$是通过已知初始值得到的像素点估计坐标。该方程的求解是一个经典的非线性优化问题，可以通过LM算法来求解。

2.5. 摄像机的畸变校正模型修正

在张氏标定法中只关注径向畸变，并且假设x方向和y方向上的径向畸变是一致的。
由于在实际情况中，径向畸变较小，所以可以利用主点及其周围的泰勒级数来表示。在张氏标定法中，利用泰勒展开的前两项来确定径向畸变的畸变洗漱。数学表达式如下：
$$\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\smile$}}\over
u} = u + (u - u_0)[k_1({x^2} + {y^2}) + k_2({x^2} + {y^2})^2] \tag{31}$$
$$\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\smile$}}\over
v} = v + (v - v_0)[k_1({x^2} + {y^2}) + k_2({x^2} + {y^2})^2] \tag{32}$$
其中，$(u,v)$代表理想无畸变的像素坐标，$(\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\smile$}}\over
u},\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\smile$}}\over
v})$代表实际径向畸变情况下的像素坐标，$(u_0,v_0)$代表当前点，(x,y)代表理想无畸变时的连续图像坐标，k1,k2代表前两阶的畸变参数。
将其写成矩阵的形式：
$$\left[{ \matrix{
(u-u_0)({x^2}+{y^2})& (u-u_0){({x^2}+{y^2})}^2 \cr
(v-v_0)({x^2}+{y^2})& (v-v_0){({x^2}+{y^2})}^2 \cr
}}\right] \left[{\matrix{
k_1 \cr
k_2 \cr
}}\right] =\left[{\matrix{
\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\smile$}}\over
u}-u \cr
\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\smile$}}\over
v}-v \cr
}}\right] \tag{33}$$
根据节2.4. 我们有n幅图像，每幅图像都可以构造如上形式的矩阵方程，因此可以得到：
$$k=({D^T}D)^{-1}{D^T}d \tag{34}$$
其中，D是矩阵左边所有系数构成的系数矩阵，d是等式右边的有畸变像素和无畸变像素坐标之差构成的矩阵。运用最小二乘法对结果进行优化，就可以得到径向畸变$k=[k_1,k_2]$。
将该径向畸变融入节2.4.为求解最优化而构造的能量函数中，得到：
$$\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^m {||m_{ij} - \hat m(A,k_1,k_2,R_i,t_i,M_j)||^2} } \tag{35}$$
对上式求解LM得到所有未知参数的最优解。

2.6. 张氏标定法步骤总结

张氏标定法的流程如下所示：

1. 打印一张标定板，然后附加到一个平坦的表面上。
2. 通过移动相机或者平面拍摄关于标定板的各种角度的图片。
3. 检测图像中的特征点。
4. 计算五个内部参数和所有外部参数。
5. 通过最小二乘法先行求解径向畸变系数。
6. 通过求最小化参数值，优化所有的参数。

3. 总结

关于张氏标定法的基本原理与流程到这就基本结束了，参考了作者的原paper和网上的一些博客，对此已一一申明，同时对作者和博主表示感谢。刚开始规规矩矩的写博客，感觉还是挺受益匪浅的，继续加油吧！

转载请注明出处