SLAM入门之视觉里程计(6)：相机标定张正友经典标定法详解

想要从二维图像中获取到场景的三维信息，相机的内参数是必须的，在SLAM中，相机通常是提前标定好的。张正友于1998年在论文："A Flexible New Technique fro Camera Calibration"提出了基于单平面棋盘格的相机标定方法。该方法介于传统的标定方法和自标定方法之间，使用简单实用性强，有以下优点：

不需要额外的器材，一张打印的棋盘格即可。
标定简单，相机和标定板可以任意放置。
标定的精度高。

相机的内参数

设$P=(X,Y,Z)$为场景中的一点，在针孔相机模型中，其要经过以下几个变换，最终变为二维图像上的像点$p=(\mu,\nu)$:

将$P$从世界坐标系通过刚体变换(旋转和平移)变换到相机坐标系，这个变换过程使用的是相机间的相对位姿，也就是相机的外参数。
从相机坐标系，通过透视投影变换到相机的成像平面上的像点$p=(x,y)$。
将像点$p$从成像坐标系，通过缩放和平移变换到像素坐标系上点$p=(\mu,\nu)$。

相机将场景中的三维点变换为图像中的二维点，也就是各个坐标系变换的组合，可将上面的变换过程整理为矩阵相乘的形式：

\[s\left(\begin{array}{c}\mu\\ \nu \\ 1\end{array}\right) = \left[\begin{array}{ccc}\alpha & 0 &c_x \\ 0 & \beta & c_y \\ 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}R&t\\0^T&1\end{array}\right] \left(\begin{array}{c}X\\Y\\Z\\1\end{array}\right) \\
= \left[\begin{array}{cccC}f_x&0&c_x&0\\0&f_y&c_y&0\\0&0&1&0\end{array}\right]\left[\begin{array}{cc}R&t\\0^T&1\end{array}\right] \left(\begin{array}{c}X\\Y\\Z\\1\end{array}\right)
\]

将矩阵$K$称为相机的内参数,

\[K = \left[\begin{array}{ccc}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{array}\right]
\]

其中，$\alpha,\beta$表示图像上单位距离上像素的个数，则$f_x = \alpha f,f_y = \beta f$将相机的焦距$f$变换为在x,y方向上像素度量表示。

另外，为了不失一般性，可以在相机的内参矩阵上添加一个扭曲参数$\gamma$，该参数用来表示像素坐标系两个坐标轴的扭曲。则内参数$K$变为

\[K = \left[\begin{array}{ccc}f_x&\gamma&c_x\\0&f_y&c_y\\0&0&1\end{array}\right]
\]

对于大多数标准相机来说，可将扭曲参数$\gamma$设为0. Multiple View Geometry in Computer Vision

张氏标定法

在上一篇博文单应矩阵，介绍的单应矩阵表示两个平面间的映射。在张氏标定法中，用于标定的棋盘格是三维场景中的一个平面$\Pi$，其在成像平面的像是另一个平面$\pi$，知道了两个平面的对应点的坐标，就可以求解得到两个平面的单应矩阵$H$。其中，标定的棋盘格是特制的，其角点的坐标是已知的；图像中的角点，可以通过角点提取算法得到，这样就可以得到棋盘平面$\Pi$和图像平面$\pi$的单应矩阵$H$。

通过上面的相机模型有：$$p = K[R|t]P$$

其中$p$是像点坐标，$P$是标定的棋盘坐标。这样就可以得到下面的等式：

\[H = K[R|t]
\]

$H$表示的是成像平面和标定棋盘平面之间的单应矩阵。通过对应的点对解得$H$后，则可以通过上面的等式得到相机的内参数$K$，以及外参旋转矩阵$R$和平移向量$t$。

棋盘平面和成像平面间的单应

将一个平面映射到另一个平面，将棋盘格所在的平面映射到相机的成像平面，则有

\[p = HP
\]

$p$为棋盘格所成像的像点坐标，$P$棋盘格角点在世界坐标系的坐标。

设棋盘格所在的平面为世界坐标系中$Z = 0$的平面，这样棋盘格的任一角点$P$的世界坐标为$(X,Y,0)$，根据小孔相机模型：

\[s\left(
\begin{array}{c} u \\ v \\1 \end{array} \right) = K\left[\begin{array}{c}R&t\end{array}\right]\left(
\begin{array}{c} X \\ Y \\ 0 \\ 1 \end{array} \right) = K[\begin{array}{c}r_1&r_2&r_3&t\end{array}]\left(\begin{array}{c} X \\ Y \\ 0 \\ 1 \end{array} \right)
= K[\begin{array}{c}r_1&r_2&t\end{array}]\left(\begin{array}{c} X \\ Y \\ 1 \end{array} \right)
\]

根据平面间的单应性，有

\[s\left(
\begin{array}{c} u \\ v \\1 \end{array} \right) = H \left(\begin{array}{c} X \\ Y \\ 1 \end{array} \right)
\]

将上面两个等式进行整合，则可以得到单应矩阵$H$和相机矩阵（包含内参和外参）的相等，如下：

\[H = \lambda K[\begin{array}{c}r_1&r_2&t\end{array}]
\]

这样就可以使用棋盘平面和成像平面间的单应矩阵来约束相机的内参和外参。单应矩阵$H$可以通过棋盘平和成像平面上对应的点计算出来。

内参的约束条件

通过平面间的单应，可以得到如下的等式

\[H =\left[\begin{array}{c}h_1&h_2&h_3\end{array}\right]= \lambda K[\begin{array}{c}r_1&r_2&t\end{array}]
\]

将旋转矩阵$R$的各个列向量和平移向量$t$使用$H$的列向量表示，

\[\begin{array}{l}
r_1 = \lambda K^{-1}h_1 \\
r_2 = \lambda K^{-1}h_2 \\
t = \lambda K^{-1}h_3
\end{array}
\]

又由于，$R$是旋转矩阵，则其是正交矩阵，也就是其任意两个列向量的内积为0，列向量的模为1。故有：

\[\begin{array}{l}
r_1^Tr_2 = 0 \\
\Arrowvert r_1 \Arrowvert = \Arrowvert r_2 \Arrowvert = 1 \\
\end{array}
\]

则对于一幅棋盘标定版的图像（一个单应矩阵）可以获得两个对内参数的约束等式：

\[\left\{
\begin{array}{l}h_1^TK^{-T}K^{-1}h_2 = 0 \\ h_1^TK^{-T}K^{-1}h_1 = h_2^TK^{-T}K^{-1}h_2 = 1\end{array}
\right.
\]

求解内参数

通过一幅标定板的图像可以的得到关于内参数的两个等式，令

\[B=K^{-T}K^{-1}=
\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]=
\left[
\begin{array}{ccc}
\frac{1}{\alpha^2} & -\frac{\gamma}{\alpha^2\beta} & \frac{v_0\gamma-u_0\beta}{\alpha^2\beta} \\
-\frac{\gamma}{\alpha^2\beta} & \frac{\gamma^2}{\alpha^2\beta^2}+\frac{1}{\beta^2} & -\frac{\gamma(v_0\gamma-u_0\beta)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2} \\
\frac{v_0\gamma-u_0\beta}{\alpha^2\beta} & -\frac{\gamma(v_0\gamma-u_0\beta)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2} & \frac{(v_0\gamma-u_0\beta)^2}{\alpha^2\beta^2}+\frac{v_0}{\beta^2}+1
\end{array}
\right]\]

注意，矩阵$B$是一个对称矩阵，其未知量只有6个，将6个未知量写为向量的形式

\[b =\left[\begin{array}{ccc}B_{11} ,B_{12} , B_{22},B_{13},B_{23},B_{33}\end{array}\right]
\]

令$h_i$为单应矩阵$H$的第$i$个行向量，则有

\[h_i = [h_{i1},h_{i2},h_{i3}]^T
\]

故：

\[h_iK^{-T}K^{-1}h_j = h_iBh_j = v_{ij}^Tb\\其中，v_{ij}=\left[ \begin{array}{cccccc} 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]^T
\]

有了上边的等式，再来看从一幅标定板图像得到的等式

\[\left\{
\begin{array}{l}h_1^TK^{-T}K^{-1}h_2 = 0 \\ h_1^TK^{-T}K^{-1}h_1 = h_2^TK^{-T}K^{-1}h_2 = 1\end{array}
\right. \Rightarrow \left\{ \begin{array}{l}v_{22}^Tb = 0 \\ v_{11}b = v_{12}b \end{array}\right.
\]

写成矩阵的形式有：

\[\left[ \begin{array}{c} v_{12}^T \\ v_{11}-v_{22} \end{array}\right] b = 0
\]

上面的一幅标定板图像取得的约束等式，假如有$n$幅图像，则

\[Vb = 0
\]

其中，$V$是一个$2n\times 6$的矩阵，$b$是一个6维向量，所以

当$n \ge 3$，可以得到$b$的唯一解;
当$n = 2$，则可以假设扭曲参数$\gamma=0$作为额外的约束条件
当$n = 1$，则值能计算两个相机的内参数

对于方程$Vb = 0$可以使用SVD求得其最小二乘解。对$V^TV$进行SVD分解，其最小特征值对应的特征向量就是$Vb = 0$的最小二乘解，从而求得矩阵$B$。由于这里得到的$B$的估计值是在相差一个常量因子下得到的，所以有：

\[B = \lambda A^{-T}A
\]

从而可以得到相机的各个内参数：

\[\left\{
\begin{array}{l}
c_x = \gamma c_y /\beta - B_{13}\alpha^2 / \lambda \\
c_y = (B_{12}B_{13}-B_{11}B_{23})/(B_{11}B_{22}-B_{12}^2) \\
\alpha = \sqrt{\lambda /B_{11}} \\
\beta = \sqrt{\lambda B_{11}/(B_{11}B_{22}-B_{12}^2)} \\
\gamma = -B_{12}\alpha^2\beta / \lambda \\
\lambda = B_{33}-[B_{13}^2 + c_y(B_{12}B_{13}-B_{11}B_{23})]/B_{11}
\end{array}
\right.
\]

最大似然估计

上面使用最小二乘法得到估计得到的解，并没有物理上的实际意义，。为了进一步增加标定结果的可靠性，可以使用最大似然估计(Maximum likelihood estimation)来优化上面估计得到的结果。

假设同一相机从$n$个不同的角度的得到了$n$幅标定板的图像，每幅图像上有$m$个像点。$M_{ij}$表示第$i$幅图像上第$j$个像点对应的标定板上的三维点，则

\[\hat{m}(K,R_i,t_i,M_{ij}) = K[\begin{array}{c}R&t\end{array}]M_{ij}
\]

$\hat{m}(K,R_i,t_i,M_{ij})$表示$M_{ij}$的像点。其中，$R_i,t_i$表示第$i$幅图像对应相机的旋转矩阵和平移向量，$K$是相机的内参数。则像点$m_{ij}$的概率密度函数是

\[f(m_{ij})=\frac{1}{\sqrt{2\pi}}e^{\frac{-(\hat{m}(K,R_i,t_i,M_{ij})-m_{ij})^2}{\sigma^2}}
\]

构造似然函数

\[L(A,R_i,t_i,M_{ij}) = \prod^{n,m}_{i=1,j=1}f(m_{ij})=\frac{1}{\sqrt{2\pi}}e^{\frac{-\sum^n_{i=1}\sum^m_{j=1}(\hat{m}(K,R_i,t_i,M_{ij})-m_{ij})^2}{\sigma^2}}
\]

为了能够让$L$取得最大值，需要最小化下面的值

\[\sum^n_{i=1}\sum^m_{j=1} \| \hat{m}(K,R_i,t_i,M_{ij})-m_{ij} \|^2
\]

这是一个非线性优化问题，可以使用Levenberg-Marquardt的方法，利用上面得到的解作为初始值，迭代得到最优解。

消除径向畸变

为了取得好的成像效果，通常要在相机的镜头前添加透镜。在相机成像的过程中，透镜会对光线的传播产生影响，从而影响相机的成像效果，产生畸变：

透镜自身的形状对才光线的传播产生影响，形成的畸变称为径向畸变。在小孔模型中，一条指向在成像平面上的像仍然是直线。但是在实际拍摄的过程中，由于透镜的存在，往往将一条直线投影成了曲线，越靠近图像的边缘，这种现象越明显。透镜往往是中心对称的，使得这种不规则的畸变通常是径向对称的。主要有两大类：桶形畸变和枕形畸变。如下图
由于在相机组装的过程中，透镜不能和成像平面严格平行，会引入切向畸变。

张氏标定法中只关注了影响较大的径向畸变。

设，$(\mu,\nu)$是理想的无畸变的像素坐标；$(\hat{\mu},\hat{\nu})$是畸变后的像素坐标；$(\mu_0,\nu_0)$是相机的主点；$(x,y)$和$(\hat{x},\hat{y})$理想的无畸变的归一化的图像坐标和畸变后的归一化图像坐标，使用下面的式子表示径向畸变：

\[\hat{x} = x + x[k_1(x^2 + y^2) + k_2(x^2 + y^2)^2] \\
\hat{y} = y + y[k_1(x^2 + y^2) + k_2(x^2 + y^2)^2]
\]

$k_1,k_2$表示径向畸变的系数。径向畸变的中心和相机的主心是在相同的位置，

\[\begin{array}{l}
\hat{\mu} = \mu_0 + \alpha \hat{x} + \gamma \hat{y} \\
\hat{\nu} = \nu_0 + \beta \hat{y} \\
\end{array}
\]

假设$\gamma = 0$，则有：

\[\hat \mu = \mu + (\mu-\mu_0)[k_1(x^2+y^2)+k_2(x^2+y^2)^2] \\
\hat \nu = \nu + (\nu-\nu_0)[k_1(x^2+y^2)+k_2(x^2+y^2)^2]
\]

将上面的式子改写为矩阵的形式

\[\left[
\begin{array}{cc}
(\mu-\mu_0)(x^2+y^2) & (\mu-\mu_0)(x^2+y^2)^2 \\
(\nu-\nu_0)(x^2+y^2) & (\nu-\nu_0)(x^2+y^2)^2
\end{array}
\right]
\left[
\begin{array}{c}
k_1 \\ k_2
\end{array}
\right]=
\left[
\begin{array}{c}
\hat \mu -\mu \\ \hat \nu -\nu
\end{array}
\right]
\]

上面的等式是从一幅图像上的一个点取得，设有$n$幅图像，每幅图像上有$m$个点，则将得到的所有等式组合起来，可以得到$2mn$个等式，将其记着矩阵形式

\[Dk = d
\]

则可得

\[k=[k_1\ k_2]^T = (D^TD)^{-1}D^Td \]

和上面类似利用最大似然估计取得最优解，使用LM的方法估计使得下面式子是最小值的参数值

\[\sum^n_{i=1}\sum^m_{j=1} \| \hat{m}(K,k_1,k_2,R_i,t_i,M_{ij})-m_{ij} \|^2
\]

得到畸变参数$k_1,k_2$后，可以先将图像进行去畸变处理，然后用去畸变后的图像坐标估计相机的内参数。

OpenCV中的张正友相机标定

OpenCV中提供了对张正友标定算法的实现，其使用步骤如下：

制作标定使用标定板。标定板很简单，如下图：

可以将其打印下来固定到一张平板上就是标定使用的标定板。使用同一相机从不同的位置，不同的角度，不同的姿态，拍摄标定板的多张照片（一般不少于3张，以10-20张为宜）。

提取标定板的内角点的世界坐标，这里需要注意标定板的大小是标定板在水平和竖直方向上内焦点的个数。内焦点指的是，标定板上不挨着表姐的角点。比如上图制作的标定板的大小是$(4,6)$，其在水平方向有4个内焦点；竖直方向有6个内焦点。标定板所在的世界坐标系为$Z=0$，则其角点的世界坐标系如下：

        // 3D场景中的点，在棋盘坐标系中初始化棋盘角点，这些角点的坐标为(x,y,z) = (i,j,0)

        // 棋盘所在的世界坐标系，X轴竖直向下，Y轴水平向右，Z = 0

        for (int i = 0; i < boardSize.height; i++)

            for (int j = 0; j < boardSize.width; j++)

                objectCorner.push_back(Point3f(i, j, 0.0f));

提取标定板的角点的图像坐标，标定板角点的提取可以调用函数findChessboardCorners(image, boardSize, imageCorner);，为了得到更高的精度，可以将提取到的像素坐标精确到亚像素。提取角点图像坐标的代码如下：

        // 2D 图像的像素坐标

        Mat image; //  保存标定板的图像

        int viewPointCount = 0;

        // 从各个视角

        for (int i = 0; i < filelist.size(); i++)

        {

            image = imread(filelist[i], 0);

            // 取得图像的角点

            bool found = findChessboardCorners(image, boardSize, imageCorner);

            // 获取亚像素精度

            cornerSubPix(image, imageCorner, Size(5, 5), Size(-1, -1),

                TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 30, 0.1));

            // 取得的角点数目满足要求，则将它加入数据中

            if (imageCorner.size() == boardSize.area())

            {

                // 将3D场景点及其对应像的像点加入

                addPoints(imageCorner, objectCorner);

                viewPointCount++;

            }

        }

已经取得了角点的世界坐标以及对应的像素坐标，就可以进行标定了，调用calibrateCamera(objectPoints, imagePoints, imageSize, camteraMatrix, distCoeffs, rvecs, tvecs, flag); 这里的cameraMatrix就是要求的相机的内参数矩阵。

上面只是简单的介绍下OpenCV相机标定的使用方法，具体的可以参考官方文档。