传感器的标定

本文介绍SLAM系统中不同传感器以及之间的标定原理与步骤。

极简相机标定步骤：

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

参考来源：一文详解相机标定算法原理

相机标定

问题引出

一. 为什么要相机标定？

​ 在图像测量过程以及机器视觉应用中，为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系，必须建立相机成像的几何模型，这些几何模型参数就是相机参数。

​ 【1】进行摄像机标定的目的：求出相机的内、外参数，以及畸变参数。
​ 【2】标定相机后通常是想做两件事：一个是由于每个镜头的畸变程度各不相同，通过相机标定可以校正这种镜头畸变矫正畸变，生成矫正后的图像；另一个是根据获得的图像重构三维场景。

摄像机标定过程，简单的可以简单的描述为通过标定板，如下图，可以得到n个对应的世界坐标三维点Xi和对应的图像坐标二维点xi，这些三维点到二维点的转换都可以通过上面提到的相机内参K，相机外参R和t，以及畸变参数D，经过一系列的矩阵变换得到。

二. 什么叫相机标定？

​ 在大多数条件下这些参数必须通过实验与计算才能得到，这个求解参数的过程就称之为相机标定（或摄像机标定）

三. 为什么相机标定很重要？

无论是在图像测量或者机器视觉应用中，相机参数的标定都是非常关键的环节，其标定结果的精度及算法的稳定性直接影响相机工作产生结果的准确性。因此，做好相机标定是做好后续工作的前提，提高标定精度是科研工作的重点所在。

四. 相机标定方法有哪些？

相机标定方法有：传统相机标定法、主动视觉相机标定方法、相机自标定法。

1. 传统相机标定法需要使用尺寸已知的标定物，通过建立标定物上坐标已知的点与其图像点之间的对应，利用一定的算法获得相机模型的内外参数。根据标定物的不同可分为三维标定物和平面型标定物。三维标定物可由单幅图像进行标定，标定精度较高，但高精密三维标定物的加工和维护较困难。平面型标定物比三维标定物制作简单，精度易保证，但标定时必须采用两幅或两幅以上的图像。传统相机标定法在标定过程中始终需要标定物，且标定物的制作精度会影响标定结果。同时有些场合不适合放置标定物也限制了传统相机标定法的应用。

2. 目前出现的自标定算法中主要是利用相机运动的约束。相机的运动约束条件太强，因此使得其在实际中并不实用。利用场景约束主要是利用场景中的一些平行或者正交的信息。其中空间平行线在相机图像平面上的交点被称为消失点，它是射影几何中一个非常重要的特征，所以很多学者研究了基于消失点的相机自标定方法。自标定方法灵活性强，可对相机进行在线定标。但由于它是基于绝对二次曲线或曲面的方法，其算法鲁棒性差。

3. 基于主动视觉的相机标定法是指已知相机的某些运动信息对相机进行标定。该方法不需要标定物，但需要控制相机做某些特殊运动，利用这种运动的特殊性可以计算出相机内部参数。基于主动视觉的相机标定法的优点是算法简单，往往能够获得线性解，故鲁棒性较高，缺点是系统的成本高、实验设备昂贵、实验条件要求高，而且不适合于运动参数未知或无法控制的场合。

相机标定的概念

相机标定就是通过输入带有标定pattern的标定板来计算相机参数，来用简单地数学模型来表达复杂的成像过程。求解这个数学模型，也就是求解相机的参数，包括相机的内参，外参以及畸变参数。有了这个数学模型，我们可以对相机拍摄的图片进行畸变校正，也可以用多个相机拍摄图片来进行三维重建，测量物体距离以及其他的计算机视觉的应用。

相机成像需要通过一系列变换组成

从世界坐标系到像素坐标系，写成矩阵形式，可以得到：
$$
\begin{array}{r}
z_{c} \cdot\left[\begin{array}{c}
u \
v \
1
\end{array}\right]=\left[\begin{array}{ccc}
\frac{1}{d x} & 0 & u_{0} \
0 & \frac{1}{d y} & v_{0} \
0 & 0 & 1
\end{array}\right]\left[\begin{array}{llll}
f & 0 & 0 & 0 \
0 & f & 0 & 0 \
0 & 0 & 1 & 0
\end{array}\right] \cdot\left[\begin{array}{cc}
R_{3 \times 3} & T_{3 \times 1} \
O & 1
\end{array}\right] \cdot\left[\begin{array}{c}
x_{w} \
y_{w} \
z_{w} \
1
\end{array}\right] \
=\mathbf{M}{1} \mathbf{M}{2}\left[\begin{array}{c}
x_{w} \
y_{w} \
z_{w} \
1
\end{array}\right]
\end{array}
$$

上式子中，$M_1$为相机内参，包括相机的教具，光轴与图像平面的角点位置等内部参数，和外部因素无关，表示为:
$$
M_1 = \left[\begin{array}{ccc}
f_x & 0 & u_{0} \
0 & f_y & v_{0} \
0 & 0 & 1
\end{array}\right]
$$
如果相机像素坐标系的坐标轴不是完全垂直的吗，会有一个偏斜系数，坐标轴不垂直的的话，偏斜系数不为0。
$$
M_1 = \left[\begin{array}{ccc}
f_x & s & u_{0} \
0 & f_y & v_{0} \
0 & 0 & 1
\end{array}\right]
$$
$M_2$为相机外参，表征世界坐标系到相机坐标系的旋转关系，是相机在世界坐标系的下的位置姿态矩阵，认为是摄像机的外参，一般情况下，如果世界坐标系设置为相机坐标系，即二者重合，那么这个外参就是一个单位矩阵。
$$
M_{2}=\left[\begin{array}{ll}
R_{3 \times 3} & T_{3 \times 1}
\end{array}\right]=\left[\begin{array}{llll}
r_{11} & r_{12} & r_{13} & t_{1} \
r_{21} & r_{22} & r_{23} & t_{2} \
r_{31} & r_{32} & r_{33} & t_{3}
\end{array}\right]
$$

镜头畸变

实际使用中，相机镜头的畸变主要包括切向畸变与径向畸变两种。

1. 径向畸变是由于相机的透镜形状造成的；在光学上，桶形畸变是由于图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反。在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
2. 切向畸变则是整个相机的组装过程中造成的；

图像的径向畸变往往被描述为一个低阶多项式模型：
$$
\begin{aligned}
&x_{\text {corrected }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \
&y_{\text {corrected }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)
\end{aligned}
$$
注意它们都是归一化平面上的点，而不是像素平面上的点。

在式（5.11）描述的纠正模型中，对于畸变较小的图像中心区域，畸变纠正主要是 $k_1$起作用。而对于畸变较大的边缘区域主要是 $k_2$起作用。普通摄像头用这两个系数就能很好的纠正径向畸变。对畸变很大的摄像头，比如鱼眼镜头，可以加入 $k_3$ 畸变项对畸变进行纠正。

另一方面，对于切向畸变，可以使用另外的两个参数 p1, p2 来进行纠正:
$$
\left{\begin{array}{l}
x_{\text {corrected }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right) \
y_{\text {corrected }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y
\end{array} .\right.
$$

相机待标定的参数

1. 与镜头畸变有关的参数;$k_1$, $k_2$，$k_3$，$p_1$，$p_2$。

   镜头畸变包括切向畸变和径向畸变两种；

2. 外参，相机坐标系与世界坐标系之间的旋转与平移R, t；

相机标定原理

单应性矩阵

平面的单应性关系定义为从一个平面到另一个平面的投影映射。从世界坐标系中物体牌面到相机坐标系的成像平面指尖的映射就是平面单应性。我们采用的是标定板，可以认为是用到的模型平面。模型平面上的一点$Q=[X,Y,Z,1]^T$映射到成像平面上的一点$q = [x,y,1]^T$，他们之间的单应性关系可以描述为:
$$
q = s H Q
$$
单应性矩阵H本来有9个元素，但由于是单应性矩阵，考虑到其次坐标，可以提出到尺度因子中，所以有8个参数需要求解，自由度是8，求解8个参数至少需要四个点，两个平面中的对应的四组，可以得到8个方程。求解方程可以得到单应性关系。采用更多的点可以减小噪音和其他不确定因素的影响。我们是完全可以在不知道内外参的 情况下求解单应性关系的。一般情况下我们会有更多的点，opencv使用findHomography接口来求解单应性，有多种求解的算法。method为0，考虑所有点，计算结果使重投影 的误差最小，即最小化均方误差。method为RANSAC，从所有点集中随机选择子集，计算需要这样的随机样本，保留具有最大内部点的样本。

相机求内参的两个基本约束

给定标定平面的一张图片，它的单应性可以被估计，假设$H = [h_1,h_2,h_3]$,由公式可以得出
$$
\left[\begin{array}{lll}
\mathrm{h}{1} & \mathrm{~h}{2} & \mathrm{~h}{3}
\end{array}\right]=\lambda \mathbf{A}\left[\begin{array}{lll}
\mathrm{r}{1} & \mathrm{r}{2} & \mathrm{t}
\end{array}\right]
$$
分解后可以得到：
$$
\mathbf{h}{1}=\lambda \cdot \mathbf{A} \cdot \mathbf{r}{1}, \mathbf{h}{2}=\lambda \cdot \mathbf{A} \cdot \mathbf{r}{2}, \mathbf{h}{3}=\lambda \cdot \mathbf{A} \cdot \mathrm{t}
$$
$$
\mathrm{r}{1}=\frac{1}{\lambda} \cdot \mathbf{A}^{-1} \cdot \mathrm{h}{1}, \mathrm{r}{2}=\frac{1}{\lambda} \cdot \mathbf{A}^{-1} \cdot \mathrm{h}{2}, \mathrm{t}=\frac{1}{\lambda} \cdot \mathbf{A}^{-1} \cdot \mathrm{h}_{3}
$$

公式中，$\lambda$是一个尺度因子。由于$r_1$和$r_2$分别是绕着x, y轴旋转的，x轴,y轴均垂直于z轴，所以是正交的。所以我们可以得到．

1. x轴与y轴正交
   $$
   \mathbf{h}{1}^{T} \mathbf{A}^{-T} \mathbf{A}^{-1} \mathbf{h}{2}=0
   $$

2. 旋转向量的模相等
   $$
   \mathbf{h}{1}^{T} \mathbf{A}^{-T} \mathbf{A}^{-1} \mathbf{h}{1}=\mathbf{h}{2}^{T} \mathbf{A}^{-T} \mathbf{A}^{-1} \mathbf{h}{2}
   $$
   对于一个给定的单应性矩阵，这是对内参的两个基础的约束条件。一个单应性矩阵 有8个自由度，六个参数（旋转3个角度，平移用三个偏移量），然而我们只得到了内参 的两个限制。接下里的章节，我们会给出一个几何解释。

相机标定内参的求解

令
$$
\begin{gathered}
\mathbf{B}=\mathbf{A}^{-T} \mathbf{A}^{-1} \equiv\left[\begin{array}{ccc}
B_{11} & B_{12} & B_{13} \
B_{21} & B_{22} & B_{23} \
B_{31} & B_{32} & B_{33} p
\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\left(v_{0} \gamma-u_{0} \beta\right)}{\alpha^{2} \beta^{2}}-\frac{v_{0}}{\beta^{2}} \
\frac{v_{0} \gamma-u_{0} \beta}{\alpha^{2} \beta} & -\frac{\gamma\left(v_{0} \gamma-u_{0} \beta\right)}{\alpha^{2} \beta^{2}}-\frac{v_{0}}{\beta^{2}} & \frac{\left(v_{0} \gamma-u_{0} \beta\right)^{2}}{\alpha^{2} \beta^{2}}+\frac{v_{0}^{0}}{\beta^{2}}+1
\end{array}\right]}
\end{gathered}
$$

注意到 $B$ 是对称的, 定义一个 6 维向量:
$$
b=\left[B_{11}, B_{12}, B_{22}, B_{13}, B_{23}, B_{33}\right]^{T}
$$
令矩阵 $H$ 的第 $i^{\text {th }}$ 个列向量为 $h_{i}=\left[h_{i 1}, h_{i 2}, h_{i 3}\right]^{T}$, 代人公式 $1.26$ 可以得到:
$$
h_{i}^{T} \mathbf{B} h_{j}=v_{i j}^{T} b
$$
式中:
$$
v_{i j}=\left[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}\right]^{T}
$$
$$
\left[\begin{array}{c}
\mathbf{v}{12}^{T} \
\left(\mathbf{v}{11}-\mathbf{v}_{22}\right)^{T}
\end{array}\right] \mathbf{b}=\mathbf{0}
$$

如果观察了 $n$ 张模型平面的图片, 可以将 $n$ 个 $1.33$ 这样的方程放在一起, 可以得到:
$$
\mathbf{V} \cdot \mathbf{b}=0
$$
公式中 $\mathbf{V}$ 是一个 $2 n \times 6$ 的矩阵 (对应 $2 n$ 个方程), $b$ 有六个参数。所以要求 $n>=3$, 至少需要三张图片, 才可以得到唯一解 $b$ 。 $H$ 矩阵是可以根据单应性关系求解出来的, 每 一组标定板的 3D 真实坐标和二维坐标点就可以一个单应性关系。 $n=3$, 即三个单应性 矩阵, 也就是拍摄三张标定板的图片。
在张正友的论文中, 对不同的平面数进行了性能测试, 即使用不同的图片数, 来做 相机标定, 比较相机参数的正确性。如图1.10从实验结果可以看出, pattern 数从 2 到 3 , 误差降低非常明显。

如果 $n=2$, 可以让畸变参数 $\gamma=0$, 那么 $[0,1,0,0,0,0] \mathbf{b}=0$,
一旦 $\mathbf{b}$ 被估计出来, 我们可以计算所有的相机内参矩阵 $\mathbf{A}$ :
$$
\begin{aligned}
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)} \
\gamma &=-B_{12} \alpha^{2} \beta / \lambda \
u_{0} &=\gamma v_{0} / \beta-B_{13} \alpha^{2} / \lambda
\end{aligned}
$$
通过内参 $A$, 便可以求解每一个视图的外参矩阵, 通过公式 $1.23$, 可得到 :
$$
$\mathbf{r}{1}=\lambda^{-1} \cdot \mathbf{A}^{-1} \cdot \mathbf{h}{1}, \mathbf{r}{2}=\lambda^{-1} \cdot \mathbf{A}^{-1} \cdot \mathbf{h}{2}, \mathbf{r}{3}=\mathbf{r}{1} \times \mathbf{r}{2}, \mathbf{t}=\lambda^{-1} \cdot \mathbf{A}^{-1} \cdot \mathbf{h}{3}
$$
当然，因为数据中噪声的存在，这么计算得到的矩阵R=“2，门]一般来说不能满足 旋转矩阵的特性。 公式L34我们可以认为是2“个方程，一般情况下，我们得到的方程数要多余未知数， 所以可以采用最小二乘法求解。

极大似然求解

张正友标定法所提供的是一套获取初始相机参数的过程。而且，上述的方法是通过最小化代数距离（最小二乘法）来获得，这种方法不具有实际的物理意义。我们可以通过极大似然估计的方法来改善它，使用张正友标定得到的相机参数进行初始化，然后进行迭代求解。 我们可以得到一个标定板平面的n张照片，在这个标定板上有m个点。假设图像上 点的噪声，服从独立同分布。可以通过最小化下列的函数来得到极大似然估计：
$$
\sum_{i=1}^{n} \sum_{j=1}^{m}\left|\mathbf{m}{i j}-\hat{\mathbf{m}}\left(\mathbf{A}, \mathbf{R}{i}, \mathbf{t}{i}, \mathbf{M}{j}\right)\right|^{2}
$$
上式中 $\hat{m}\left(\mathbf{A}, \mathbf{R}{i}, \mathbf{t}{i}, M_{j}\right)$ 是空间中的点 $\mathbf{M}_{j}$ 在图像 $i$ 中的投影点, 可以根据公式1.22计算得到。求解公式上1.42的最小值是一个非线性优化问题，可以通过levenberg-Marquardt算法解决。 我们为什么不直接适用上述优化方法，求解这个最小二乘问题呢，是因为待求解的 参数比较多，很难找到一个好的初始化参数，所以张正友标定法的重点是提供了一个好的初始化。

以上推导都是假设不存在畸变参数的情况下成立的。但是事实上，相机是存在畸变参数的，因此，张正友标定法还需要通过L-M算法对于参数进行迭代优化。

事实上，张正友标定法提供的是内参与外参矩阵的优化初始值。

张正友标定法的总逻辑：

1 在进行内参矩阵和外参矩阵的求解的时候，我们假设不存在畸变；
2 在进行畸变系数的求解的时候，我们假设求得的内参矩阵和外参矩阵是无误差的。
3 我们再通过L-M算法对于参数进行迭代优化。

求解径向畸变参数

张正友标定法

基本介绍

  ”张正友标定”是指张正友教授1998年提出的单平面棋盘格的摄像机标定方法[1]。文中提出的方法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点，而仅需使用一个打印出来的棋盘格就可以。同时也相对于自标定而言，提高了精度，便于操作。因此张氏标定法被广泛应用于计算机视觉方面。

IMU标定

相机与IMU之间的外参标定

RGBD相机的标定

参考文献

1、Zhang, Zhengyou - 《IEEE Transactions on Pattern Analysis & Machine Intelligence》 - 2000
2、J.More.Thelevenberg-marquardtalgorithm,implementationandtheory.InG.A.Watson, editor,NumericalAnalysis,LectureNotesinMathematics630.Springer-Verlag,1977.