双目视觉标定——1原理与实践

0 前言

双目视觉定位是目前机器（机器人）等领域中使用得非常广泛的视觉定位技术，双目视觉是模拟人的视觉系统利用两个不同位置的摄像头的视差来确定物体的位置。由于有需要采集两个摄像头的图像共同参与计算，所以双目相机装配要求高、标定也比单目复杂。相机的标定是定位的基础标定，本文是我学习双目视觉标定方法以及的总结文档。

1 为什么要标定？

作为初学者，我们会有疑问，什么相机需要进行标定，标定到底有什么用呢？这是因为摄像机是将三维世界投射到二维平面的设备，从三维世界的场景到二维平面的成像图像，这两者的转化存在着一种映射关系，不同的相机映射关系不同，所以他们对相同场景所成像的图像也就不同，而标定的目的就是确定相机的这种映射关系。

总体来说，通过标定能够确定相机的内参、外参、畸变系数这三类参数，而这些参数就是确定相机将三维世界转为二维图像的最主要因素；而双目相机还要确认两个摄像头之间的位置关系，这是双目用于计算物体位置的前提。

1.1相机标定要标定什么？

内参：fx，fy，cx,cy,k1,k2,k3,p1,p2
外参：Rt

常用术语
内参矩阵: Intrinsic Matrix
焦距: Focal Length
主点: Principal Point
径向畸变: Radial Distortion
切向畸变: Tangential Distortion
旋转矩阵: Rotation Matrices
平移向量: Translation Vectors
平均重投影误差: Mean Reprojection Error
重投影误差: Reprojection Errors
重投影点: Reprojected Points

2 相机模型

内参、外参、畸变系数这些东西是怎么来的？那我们首先要了解一下相机的成像原理和成像过程。相机的成像是一个物理光学现象，从光学基础来讲还是比较复杂的，但是在大多数时候我们只需要关注它最核心的部分，通过构建一些简单的模型来描叙问题的核心，从而近似的还原真实的物理现象。在相机模型中，我们两类最重要的模型就是：成像模型和畸变模型。

1. 成像模型

在成像模型里针孔模型（就是我们通常所说的小孔成像）是最常用的，它简单但有很好的描叙了相机成像的本质。下图为针孔成像示意图：
在这里插入图片描述

在相机中透镜充当了小孔的作用:

在这里插入图片描述
是一个小孔成像的模型，其中：

O点表示camera centre，即相机的中心点，也是相机坐标系的中心点；
z轴表示principal axis，即相机的主轴；
q点所在的平面表示image plane，即相机的像平面，也就是图片坐标系所在的二维平面；
O1点表示principal point，即主点，主轴与像平面相交的点；
O点到O1点的距离，也就是右边图中的f,即相机的焦距；
像平面上的x和y坐标轴是与相机坐标系上的X和Y坐标轴互相平行的；
相机坐标系是以X,Y,Z（大写）三个轴组成的且原点在O点，度量值为米（m）；
像平面坐标系是以x,y（小写）两个轴组成的且原点在O1点，度量值为米（m）；
像素坐标系一般指图片相对坐标系，在这里可以认为和像平面坐标系在一个平面上，不过原点是在图片的角上，而且度量值为像素的个数（pixel）；

在相机的成像平面上放置的是光敏传感器，这些传感器会将成像平面上连续的图像进行采样，这样最终得到的是离散化的图像。在这里就会涉及到相机的内参：成像平面两个方向上传感器单位尺寸的像素密度、成像的中心位置和焦距，这5个参数（像素密度和中心坐标都有两个方向的值）就是我们标定中要确定的主要的内参。

内参一般在相机生产后就比较固定，相对而言外参则是不固定的，因为外参表示的是相机自身在三维空间中相对于给定的世界坐标的位姿，这个位姿信息包括6个参数：三个位置参数、三个姿态参数，实际上两者就是一种线性映射关系。

2. 畸变模型

实际相机用透镜的聚光特性使得投影在成像平面上的光强度更大，成像质量会更好，但是也是由于凸镜的存在也会对图像产生不好的影响，如图像的畸变。

畸变类型和原因，主要有以下两点：

透镜自身的形状对光线传播的影响——导致径向畸变
在机械组装的过程中，透镜和成像平面不平行——导致切向畸变

在径向畸变可分为两类，如下图所示，左边为枕形畸变、右边为桶形畸变：

在这里插入图片描述

可以看到径向畸变只于图像到中心的距离有关，距中心越远畸变越大。对于径向畸变我们使用与中心距离r，相关的高次多项式模型来拟合，其中 $r^2 = x^2 + y^2$ ：

$x_distorted = x(1+k_1 r^2+k_2 r^4+k_3 r^6)$
$y_distorted = y(1+k_1 r^2+k_2 r^4+k_3 r^6)$

切向畸变如下示意图：

在这里插入图片描述

我们新增参数p1、p2建立如下方程来拟合：

$x_distorted = x+2p_1 xy+p_2 (r^2+2x^2)$
$y_distorted = y+2p_2 xy+p_1 (r^2+2y^2)$

统一两种畸变：

$x_distorted = x(1+k_1 r^2+k_2 r^4+k_3 r^6)+2p_1 xy+p_2 (r^2+2x^2)$
$y_distorted = y(1+k_1 r^2+k_2 r^4+k_3 r^6)+2p_2 xy+p_1 (r^2+2y^2)$

上面统一的畸变公式，并不是将第一种畸变的公式整体代入第二个畸变公式，而是在无畸变图像上再增加两种畸变分量，而每种畸变分量只与无畸变图像相关，所以相互之间不影响，只不过径向畸变分量是乘性的而切向畸变是加性的。

根据畸变模型的公式，我们可以看到畸变系数主要是5个：k1、k2、k3、p1、p2，当然在实际中我们可以灵活选择，如畸变较小的镜头可以去掉高次项。

3. 相机成像总结

总体上，为了用数学描叙相机的成像，即将三维世界的物体转化为数字的图像，我们一般将其分为以下四个步骤（下图源于知乎）：

在这里插入图片描述

其中刚体变换需要用到相机的外参，透视投影需要用到针孔模型和相机内参中的焦距参数，畸变校正很显然需要使用两种畸变模型和对应的畸变系数，最后数字化图像则需要使用相机内参中剩余参数。

3 双目定位几何模型

成像模型和畸变模型是对单一镜头成像分析，而对于双目摄像头来说，它在成像的基础上还需要定位，因此需要建立一个能够计算物体位置的几何模型。

双目视觉的是模拟人眼，通过视差来计算物体的距离，从而得到物体的空间坐标信息，其几何模型如下图所示：
在这里插入图片描述
这里d为左右图的横坐标之差，也称视差disparty。根据视差，在f和b已知的情况下，我们可以估计目标点离相机的深度距离。视差与深度距离成反比，视差越大，距离越近。另外由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值。可以看到，当基线越长时，双目最大能测到的距离就会变远；反之，小型双目器件则只能测量很近的距离。

虽然由视差计算深度的公式很简洁，但视差d本身的计算却比较困难，我们需要确切地知道左眼图像某个像素出现在右眼图像的哪一个位置（即对应关系）。此外如果想计算每个像素的深度，其计算量与精度都将成为问题，并且只有在图像纹理变化丰富的地方才能计算视差。

从上图可以看到，双目立体视觉的原理很简单，通过同一物体在两个图片中的视差和两个镜头光轴间的距离（基线），利用相似三角形就可以简单求出物体的距离。但是我们也要看到，此计算的基础是：两个摄像机的成像平面在同一平面上，当然为了计算简单，我们还希望两个相机的中心能水平对齐，这样我们计算视差只需要用到两个图像单个坐标方向上值。

但是在实中是不存在完全共面的两个镜头的，所以在双目相机的标定中，我们除了确定单个相机的内外参数、对图像的畸变进行校正外，还需要对进行立体校正，立体校正的目的就是将实际中非共面对准的两个图像，校正到共面对准，注意这是对图像做的，因为相机镜头已经固定了。

在这里插入图片描述
可以看到，为了将两个图像调整到同一平面且中心水平对齐（外极线校准），我们需要知道两个摄像头的相对位置关系，具体来讲就是：旋转矩阵R和平移矩阵T，然后对应将图像做旋转和平移即可。获取立体校正的参数就是立体标定要做的工作，实际上只是对每个镜头进行前面单目标定，用单目标定的参数计算得到立体校正的参数。

以标定物体上的点为参考，可以分别得到左右相机的参考坐标系的坐标表达式，综合两者可以得到从左相机坐标系到右相机坐标系的转关关系，（推导下式其实是很简单的，但是网上的一些资料很少写清楚下面这个公式，有些写出来了也是不对的）：

$R = R_r R_l^{-1}$
$T = T_r - RT_l$

其中，Rr、Tr、Rl、Tl都是通过单目标定得到的参数，这样我们就可以直接求出立体校正的参数。

到此，整个双目视觉需要标定原因、各种标定的原理就讲解完成。

4 标定实践

上面说明了其各种标定参数的作用和原理，现在看看这些参数的实际标定方法。对于双标定，我们要获取的参数是四类：内参、外参、畸变参数、立体校正参数，其中畸变参数的计算需要结合内参和外参共同优化得到，而立体校正参数可以直接由内参和外参直接计算得到，所以虽然是双目相机，但是每一个镜头各自的内参和外参的获取仍然是标定的重点。

4.1内参和外参的数学表示

前面我们分析过相机的内参和外参的物理意义和计算原理，不过当我们计算时，为了保证其数学形式的简洁，通常使用矩阵来表示。从三维世界的坐标点到数字图像中的坐标点，其映射关系用矩阵方法可以表示为：

$s_0m=K[R,T]X$

X是世界坐标系中的点坐标，m是数字图像中点的坐标，使用的是齐次坐标形式，K是相机的内参矩阵，R是相机相对世界坐标系的旋转矩阵、T是平移矩阵， $s_0$ 为缩放因子。

1. 内参矩阵

相机的内参包括：两个方向的像素密度、光轴在成像平面的中心位置、相机的焦距，从上面也可以看到我们在计算中使用的是内参矩阵（Camera Intrinsics）：

$\left(\begin{array}{cc} f_x & 0 & c_x\\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{array}\right)$

其中 $f_x$ 为在x方向焦距长度上拥有的像素个数， $f_y$ 为在y方向焦距长度上拥有的像素个数，称为摄像机在x轴和y轴方向上的尺度因子，使用它可以让内参矩阵形式更加简洁。它们可以由针孔模型和图像数字化采样直接推出：

$f_x = a_x f \\ f_y = a_y f \\$

上式中 $a_x$ 和 $a_y$ 分别表示x方向和y方向上的像素密度，f表示相机的焦距；另外 $c_x$ 为中心在x轴上的像素偏置， $c_y$ 为中心在y轴上的像素偏置。至此内参矩阵就讲所有相机内部参数都包含进去了。

但是在实际中，由于制造工艺的限制，传感器上横向的像素和竖向的像素排列不是绝对的垂直，所以在内参中我们需要加入一个倾斜因子s：

$\left(\begin{array}{cc} f_x & s & u_0\\ 0 & f_y & v_0 \\ 0 & 0 & 1 \end{array}\right)$

不过需要注意的是，由于加入的倾斜因子，内参矩阵中个元素与相机各个参数的对应关系都要将倾斜因子考虑进去。

2. 旋转和平移的齐次表示

R和T是相机的外参，对应相机相对世界坐标系的旋转和平移，这里可以直接用刚体的坐标表换来计算，旋转用旋转矩阵R表示，平移用平移向量T来表示。不过有时候使用齐次矩阵统一旋转矩阵和平移向量能够让计算更简洁方便。

4.2 张正友标定法

1. 简介

相机标定主要有传统标定方法和自标定方法两类：

传统标定方法需要标定参照物，参照物的参数已知，然后分析拍摄到的参照物图像，求得相机参数，传统方法操作相对复杂，但精度较高
自标定方法不依赖于标定参照物，只领用摄像机的运动约束或者环境的约束来进行标定，自标定方法灵活方便，但由于是非线性标定，精度和鲁棒性都不高

张正有平面标定法也称张氏标定法，属于传统的标定方法。张氏标定只要求从不同的角度对同一平面拍摄2幅以上的的图像，就可以求出摄像机的内外参数。由于其平面模板（棋盘格）制作简单且不需要知道平面移动的具体的位置信息，即有穿透方法的高精度的优点、又相比其他传统的标定法要简单灵活，因此在业界得到了广泛使用。

标定所用的棋盘格如下,一般会将其打印出来贴在平面木板上当做标定物：

在这里插入图片描述

2. 单应性映射

由于我们的标定物是棋盘平面，所以棋盘平面的成像实际上是单应性映射（Homography）。那什么是单应性映射？单应性变换是同一物体在不同平面的投影，那么一个投影平面到另一个投影平面的映射关系就是单应性映射。由于单应性映射是在普通的线性变换的基础上增加了额外的约束条件，所以单应性映射会更简单：由于两个被物体都是平面，所以我们可以把变换前后的两个坐标系都建立在平面之上，那么前后两个平面的坐标都可以只用两个坐标值表示。两个平面的单应性映射关系用单应性矩阵来描叙，在张正友标定法中我们直接求得的是单应性矩阵。

在这里插入图片描述

单应性映射矩阵方程为：

$\left(\begin{array}{cc} u \\ v \\ 1 \end{array}\right) = H\left(\begin{array}{cc} x\\ y \\ 1 \end{array}\right)$

可以看到，前后连个平面的点都只用了两个坐标值的齐次坐标表示，其中H为3x3单应性矩阵，9个元素值全部未知；不过由于是齐次方程，通常将H提取一个比例系数，让其最后一个元素值为1，只需求得8个未知数即可。每一个点对对应一个矩阵方程，一个矩阵方程对应三个方程组，但是由于是齐次方程，需要去除一个比例系数，实际上一个单应性矩阵方程只能提供两个有效的方程组，因此为了求解H，所以至少需要四个对应点。

2. 从单应性矩阵计算相机内参与外参

结合本文最开始的单个相机的成像坐标转换公式，使用单应性变换，则单应性矩阵为：

$H = [h 1 h 2 h 3] = λ K [r 1 r 2 t]$

其中K仍然是内参，λ为比例系数，r1、r2、t为外参矩阵的一部分，计算将H拆解为（h1, h2, h3）的向量，取与r1和r2相关的方程，然后结合r1和r2的两个约束条件：

r1和r2位正交
r1和r2都为单位向量（模为1）

可以得到如下两个消除了r1和r2，用来计算内参K的方程组：

$h_1^T K^{-T} K^{-1} h_2 = 0 \\ h_1^T K^{-T} K^{-1} h_1 = h_2^T K^{-T} K^{-1} h_2$

内参矩阵K有5个未知量（加入了倾斜因子），每一个单应性矩阵只能得到两个上面的方程组，所以至少需要3个不同的单应性矩阵，即相机需要拍摄棋盘格三种不同的姿态。

得到了内参矩阵后，外参可以直接通过以下公式推出（由之前的推导得到）：

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

3. 径向畸变估计

张氏标定只关注了影响大的径向畸变，根据径向畸变模型，我们有联系畸变后的联系畸变后的坐标和畸变前坐标的方程。一般为避免解非线性方程组，我们使用多个图像多个角点，使用最小二乘法，以畸变前后的坐标差异最小化为目标，迭代优化。

4. 用最大似然估计优化参数

在上面推导的是理想的结果，在实际成像中会受到噪声的干扰，我们认为噪声服从高斯分布，所以在张氏标定中我们用高斯分布来拟合，然后用最大似然法估计内外参数的最优结果。

具体做法是：

求出理想的内参值
拍摄多幅棋盘图像，用理想的内参求出不同姿态图像的外参
用理想的内参和外参预测每个图像中的每个角点的成像坐标，用实际拍摄得到的坐标作为真值，可以得到每一个预测值在高斯分布下的可能的概率。
使用极大似然法，将所有的预测概率相乘得到似然函数
以理想内参为初值，迭代优化，让似然函数取得最大值

中间的数学推导我就不写了，简化后最终优化的函数如下：

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

上式需要让其取最小值，优化算法一般用Levenberg-Marquardt算法（一种介于牛顿法与梯度下降法之间的一种非线性优化方法）迭代求解。

4.3 总结

总结整个张氏标定过程包括如下步骤：

打印一张棋盘格，把它贴在一个平面上，作为标定物
通过调整标定物或摄像机的方向，为标定物拍摄一些不同方向的照片
从照片中提取棋盘格角点
估算理想无畸变的情况下，五个内参和六个外参
应用最小二乘法估算实际存在径向畸变下的畸变系数
极大似然法，优化估计，提升估计精度

第1步：准备一张棋盘格，粘贴于墙面，并用直尺测量黑白方格的真实物理长度。

第2步：调用双目摄像头，分别从不同角度拍摄得到一系列棋盘格图像。

第3步：利用左目图片数据集，进行左目相机标定，得到左目内参矩阵K1、左目畸变系数向量D1。

第4步：利用右目图片数据集，进行右目相机标定，得到右目内参矩阵K2、右目畸变系数向量D2。

第5步：将左右目测量得到的参数K1、K2、D1、D2作为输入，再同时利用左右目一一对应好的棋盘格图片，调用stereoCalibrate函数，输出左右目的旋转矩阵R、平移向量T。

第6步：基于测量得到的K1、K2、D1、D2、R、T，进行双目视觉的图像校正。

备注：需要用直尺测量黑白方格的真实物理长度，因为我们会将真实世界的棋盘格3D坐标，以相同的尺度(米为单位)，存储于object_points容器中，这样求解得到的平移向量T才会有意义，它的尺度才会和真实世界尺度相对应。然后我们会借助OpenCV棋盘格检测函数，将每张图片对应的棋盘格坐标索引，对应存储于left_img_points、 right_img_points容器中。在得到一系列对应好的2D、3D坐标点后，就可通过线性方程求解参数估计。