opencv-张氏标定法(中篇)

本文主要是介绍opencv-张氏标定法(中篇)，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

续上篇
这里写图片描述
原文链接如下：
从零开始学习「张氏相机标定法」（二）单应矩阵
从零开始学习「张氏相机标定法」（三）推导求解
当然还有玉米系列博客

标定在整个基于标定摄像机的三维重建的几何过程占有最重要最核心的地位。如下图：

从图中明显可以看出，标定得到的内参、外参和畸变系数，是双目视觉进行图片矫正，摄像机校正和3D恢复的基础。没有好的标定，双目视觉系统就无法完成3D重建。

前篇中我们已经得到了像素坐标系和世界坐标系下的坐标映射关系：

⎡ ⎣ ⎢ u v 1 ⎤ ⎦ ⎥ = s ⎡ ⎣ ⎢ f x 00 γ f y 0 u 0 v 0 1 ⎤ ⎦ ⎥ [r 1 r 2 t] ⎡ ⎣ ⎢ x W y W 1 ⎤ ⎦ ⎥ (1)

$\left[\begin{array}{c}u\\ v\\1\end{array}\right]= s\left[\begin{array}{ccc} f_x&\gamma&u_0\\ 0&f_y&v_0\\ 0&0&1\end{array}\right] \left[\begin{array}{c} r_1&r_2&t \end{array}\right] \left[\begin{array}{c} x_W\\y_W\\1\end{array}\right] \tag{1}$
其中，

u、v u 、 v $u、v$ 表示像素坐标系中的坐标，s表示尺度因子，

fx、fy、u0、v0、γ f x 、 f y 、 u 0 、 v 0 、 γ $f_x、f_y、u_0、v_0、\gamma$ （由于制造误差产生的两个坐标轴偏斜参数，通常很小）表示5个相机内参，

R,t R , t $R,t$ 表示相机外参，

XW、YW、ZW X W 、 Y W 、 Z W $X_W、Y_W、Z_W$ （假设标定棋盘位于世界坐标系中

ZW=0 Z W = 0 $Z_W=0$ 的平面）表示世界坐标系中的坐标。

1.单应性变换(Homography)

单应性变换(Homography)：在计算机视觉中被定义为一个平面到另一个平面的投影映射。
因为张氏标定是一种基于平面棋盘格的标定，所以想要搞懂张氏标定，首先应该从两个平面的单应性(homography)映射开始着手。
同样也可以简单的理解为它用来描述物体在世界坐标系和像素坐标系之间的位置映射关系。对应的变换矩阵称为单应性矩阵。在上述式子(1)中，单应性矩阵定义为：

H = s ⎡ ⎣ ⎢ f x 00 γ f y 0 u 0 v 0 1 ⎤ ⎦ ⎥ [r 1 r 2 t] = s M [r 1 r 2 t]

$H=s\left[\begin{array}{ccc} f_x&\gamma&u_0\\ 0&f_y&v_0\\ 0&0&1\end{array}\right] \left[\begin{array}{c} r_1&r_2&t\end{array}\right]= sM\left[\begin{array}{c} r_1&r_2&t \end{array}\right]$
其中，M是内参矩阵

M = ⎡ ⎣ ⎢ f x 00 γ f y 0 u 0 v 0 1 ⎤ ⎦ ⎥

$M = \left[\begin{array}{ccc} f_x&\gamma&u_0\\ 0&f_y&v_0\\ 0&0&1\end{array}\right]$
从单应矩阵定义式子来看，它同时包含了相机内参和外参。在进一步介绍相机标定知识之前，我们重点来了解一下单应性，这有助于深入理解相机标定。因为在计算机视觉领域，单应性是一个非常重要的概念。

首先，我们假设两张图像中的对应点对齐次坐标为 $(x',y',1)$ 和 $(x,y,1)$ ，单应矩阵H定义为：

H = ⎡ ⎣ ⎢ h 11 h 21 h 21 h 12 h 22 h 22 h 13 h 23 h 23 ⎤ ⎦ ⎥

$H=\left[\begin{array}{ccc} h_{11}&h_{12}&h_{13}\\ h_{21}&h_{22}&h_{23}\\ h_{21}&h_{2_2}&h_{23} \end{array}\right]$
则有：

⎡ ⎣ ⎢ x' y' 1 ⎤ ⎦ ⎥ = H = ⎡ ⎣ ⎢ h 11 h 21 h 21 h 12 h 22 h 22 h 13 h 23 h 23 ⎤ ⎦ ⎥ ⎡ ⎣ ⎢ x y 1 ⎤ ⎦ ⎥

$\left[\begin{array}{c} x'\\y'\\1\end{array}\right]= H=\left[\begin{array}{ccc} h_{11}&h_{12}&h_{13}\\ h_{21}&h_{22}&h_{23}\\ h_{21}&h_{2_2}&h_{23} \end{array}\right]\left[\begin{array}{c} x\\y\\1\end{array}\right]$
矩阵展开后有3个等式，将第3个等式代入前两个等式中可得：

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

$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}}$
也就是说，一个点对对应两个等式。在此插入一个讨论： 单应矩阵H有几个自由度？

或许有人会说，9个啊，H矩阵不是9个参数吗？从 $h_{11}$ 到 $h_{33}$ 总共9个。真的是这样吗？实际上并不是，因为这里使用的是齐次坐标系，也就是说可以进行任意尺度的缩放。比如我们把 $h_{ij}$ 乘以任意一个非零常数k并不改变等式结果：

x' = k h 11 x + k h 12 y + k h 13 k h 31 x + k h 32 y + k h 33 y' = k h 21 x + k h 22 y + k h 23 k h 31 x + k h 32 y + k h 33

$x' = \frac{kh_{11}x+kh_{12}y+kh_{13}}{kh_{31}x+kh_{32}y+kh_{33}} \\y'= \frac{kh_{21}x+kh_{22}y+kh_{23}}{kh_{31}x+kh_{32}y+kh_{33}}$
等价于：

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

$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}}$
所以实际上单应矩阵H只有8个自由度。8自由度下H计算过程有两种方法。

第一种方法：直接设置 $h_{33}$ =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}$

第二种方法：将H添加约束条件，将H矩阵模变为1，如下：

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 h 211 + h 212 + h 213 + h 221 + h 222 + h 223 + h 231 + h 232 + h 233 = 1

$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}} \\ h_{11}^2+h_{12}^2+h_{13}^2+h_{21}^2 +h_{22}^2+h_{23}^2+h_{31}^2+h_{32}^2+ h_{33}^2=1$

以第2种方法（用第1种也类似）为例继续推导，我们将如下等式（包含||H||=1约束）:

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

$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}}$
乘以分母展开，得到：

(h 31 x + h 32 y + h 33) 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})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 11 x + h 12 y + h 13 - h 31 x x' - h 32 y x' - h 33 x' = 0 h 21 x + h 22 y + h 23 - h 31 x y' + h 32 y y' + h 33 y' = 0

$h_{11}x+h_{12}y+h_{13}-h_{31}xx'-h_{32}yx'-h_{33}x'=0\\h_{21}x+h_{22}y+h_{23}- h_{31}xy'+h_{32}yy'+h_{33}y' = 0$
假如我们得到了两幅图片中对应的N个点对（特征点匹配对），那么可以得到如下线性方程组：

写成矩阵形式：

由于单应矩阵H包含了||H||=1约束，因此根据上图的线性方程组，8自由度的H我们至少需要4对对应的点才能计算出单应矩阵。这也回答了前面图像校正中提到的为何至少需要4个点对的根本原因。

但是，以上只是理论推导，在真实的应用场景中，我们计算的点对中都会包含噪声。比如点的位置偏差几个像素，甚至出现特征点对误匹配的现象，如果只使用4个点对来计算单应矩阵，那会出现很大的误差。因此，为了使得计算更精确，一般都会使用远大于4个点对来计算单应矩阵。另外上述方程组采用直接线性解法通常很难得到最优解，所以实际使用中一般会用其他优化方法，如奇异值分解、Levenberg-Marquarat（LM）算法（这个后续会更新）等进行求解。
2.相机标定如何计算单应性矩阵
经过前面一系列的介绍，我们应该大致明白如何根据打印的棋盘标定图和拍摄的照片来计算单应矩阵H。我们来总结一下大致过程。

打印一张棋盘格标定图纸，将其贴在平面物体的表面.
拍摄一组不同方向棋盘格的图片，可以通过移动相机来实现，也可以移动标定图片来实现。
对于每张拍摄的棋盘图片，检测图片中所有棋盘格的特征点（角点，也就是下图中黑白棋盘交叉点，中间品红色的圆圈内就是一个角点）。我们定义打印的棋盘图纸位于世界坐标系Zw=0的平面上，世界坐标系的原点位于棋盘图纸的固定一角（比如下图中黄色点）。像素坐标系原点位于图片左上角。
因为棋盘标定图纸中所有角点的空间坐标是已知的，这些角点对应在拍摄的标定图片中的角点的像素坐标也是已知的，如果我们得到这样的N>=4个匹配点对（越多计算结果越鲁棒），就可以根据LM等优化方法得到其单应矩阵H。

3.得到了单应矩阵，下一步就是如何求相机内外参数？
我们知道H是内参矩阵和外参矩阵的混合体，而我们想要最终分别获得内参和外参。所以需要想个办法，先把内参求出来（先求内参是因为更容易），得到内参后，外参也就随之解出了。

我们先不考虑镜头畸变，来看看如何求解内参和外参。求解思路是利用旋转向量的约束关系，以下是具体推导，建议自己演算一遍，加深理解。

为了利用旋转向量之间的约束关系，我们先将单应性矩阵H化为3个列向量，即H=[h1 h2 h3]，则有

H = [h 1 h 2 h 3] = s M [r 1 r 2 t]

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

按元素对应关系可得：

因为 旋转向量在构造中是相互正交的，即r1和r2相互正交，由此我们就可以利用“正交”的两个含义，得出每个单应矩阵提供的两个约束条件：

约束条件1：旋转向量点积为0（两垂直平面上的旋转向量互相垂直），即：

约束条件2：旋转向量长度相等（旋转不改变尺度），即：
这里写图片描述

所以一个单应性矩阵H可以提供上述两个约束条件。那么如何利用上述两个约束条件求解内参或者外参呢？我们一步一步来看，由前面可知内参矩阵M：

M = ⎡ ⎣ ⎢ f x 00 γ f y 0 u 0 v 0 1 ⎤ ⎦ ⎥

$M = \left[\begin{array}{ccc} f_x&\gamma&u_0\\ 0&f_y&v_0\\0&0&1\end{array}\right]$
记：

B = (M - 1) T M - 1 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 1 f 2 x - γ f 2 x f y v 0 γ - u 0 f y f 2 x f y - γ f 2 x f y γ 2 f 2 x f 2 y - γ v 0 γ - u 0 f y f 2 x f 2 y - v 0 f 2 y v 0 γ - u 0 f y f 2 x f y - γ v 0 γ - u 0 f y f 2 x f 2 y - v 0 f 2 y ( v 0 γ - u 0 f y ) 2 f 2 x f 2 y + v 2 0 f 2 y + 1 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ B 11 B 21 B 31 B 12 B 22 B 32 B 13 B 23 B 33 ⎤ ⎦ ⎥

$B = (M^{-1})^TM^{-1}= \left[\begin{array}{ccc} \frac{1}{f_x^2}&-\frac{\gamma}{f_x^2f_y}& \frac{v_0\gamma-u_0f_y}{f_x^2f_y}\\ -\frac{\gamma}{f_x^2f_y}&\frac{\gamma^2}{f_x^2f_y^2}&-\gamma\frac{v_0\gamma-u_0f_y}{f_x^2f_y^2}-\frac{v_0}{f_y^2}\\ \frac{v_0\gamma-u_0f_y}{f_x^2f_y}&-\gamma\frac{v_0\gamma-u_0f_y}{f_x^2f_y^2}-\frac{v_0}{f_y^2}&\frac{(v0_\gamma-u_0f_y)^2}{f_x^2f_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]$
我们看到B为对称矩阵，真正有用的元素只有6个（主对角线任意一侧的6个元素）。把B带入前面两个约束条件后可转化为：

{h T 1 B h 2 = 0 h T 1 B h 1 = h T 2 B h 2

$\begin{cases} h_1^TBh_2=0\\ h_1^TBh_1 = h_2^TBh_2 \end{cases}$
上面两约束中的式子均可写为通式

h T i B h j

$h_i^TBh_j$

的形式，定义3X3的单应矩阵 $H = \left[\begin{array}{c} h_1&h_2&h_3\end{array}\right]$ 的第 $i$ 列列向量：

h_{i} = [\begin{matrix} h_{i 1} & h_{i 2} & h_{i 3} \end{matrix}]

$h_i = \left[\begin{array}{c} h_{i1}&h_{i2}&h_{i3}\end{array}\right]$

将如下表达式代入上述的约束单项式：

h T i B h 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 11 B 12 B 22 B 13 B 23 B 33 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$h_i^TBh_j=\left[\begin{array}{c} 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]\left[ \begin{array}{c}B_{11}\\B_{12}\\ B_{22}\\B_{13}\\B_{23}\\B_{33}\end{array}\right]$

为了简化表达形式，令：

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_{ij}=\left[\begin{array}{c} 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], b=\left[ \begin{array}{c}B_{11}\\B_{12}\\ B_{22}\\B_{13}\\B_{23}\\B_{33}\end{array}\right]$
则有：

h T i B h j = v T i j b

$h_i^TBh_j=v_{ij}^Tb$
由此，两约束条件最终可以转化为如下形式：

如果我们拍摄了n张不同角度的标定图片，因为每张图片我们都可以得到一组（2个）上述的等式。其中，v12,v11,v22可以通过前面已经计算好的单应矩阵得到，因此是已知的，而b中6个元素是待求的未知数。因此，至少需要保证图片数 n>=3，我们才能解出b。

根据n张不同角度的标定图片，最终我们得到了一个矩阵集合 Vb=0 ，其中V是一个 (2n x 6) 的矩阵。如果 n>=3，就可以得到唯一解b（带有一个比例因子）。

如果 n=2，也就是只有两张标定图片，那么我们可以设置内参中的γ=0（γ表示由于制造误差产生的两个坐标轴偏斜参数，通常很小，可忽略），将前面式子（搬运到下图）中γ=0可以看到对应 B12=0，换句话说，就是增加了一个约束条件：[0, 1, 0, 0, 0, 0]b = 0。

如果n=1，只能假设u0, v0已知（位于图像中心）且 γ=0，这样只能解出fx, fy两个参数。

前面说到，B中包含一个尺度因子λ，即：

B = λ M - T M

$B = \lambda M^{-T}M$

假设我们已经根据前面计算得到了矩阵B元素的值，那么根据已知的矩阵B很容易解出内参，如下：

⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ f x = λ B 11 - - - \sqrt f y = λ B 11 ( B 11 B 22 - B 2 12 ) - - - - - - - - - \sqrt u 0 = γ v 0 f y - B 13 f 2 x λ v 0 = B 12 B 13 - B 11 B 23 B 11 B 22 - B 2 12 γ = B 12 f 2 x f y λ λ = B 33 - [ B 2 13 + v 0 ( B 12 B 13 - B 11 B 23 ) ] B 1 1

$\begin{cases} f_x = \sqrt{\frac{\lambda}{B_{11}}}\\ f_y = \sqrt{\frac{\lambda B_{11}}{(B_{11}B_{22}-B_{12}^2)}}\\ u_0 = \frac{\gamma v_0}{f_y}-\frac{B_{13}f_x^2}{\lambda}\\ v_0 = \frac{B_{12}B_{13}-B_{11}B_{23}}{B_{11}B_{22}-B_{12}^2}\\ \gamma = \frac{B_{12}f_x^2f_y}{\lambda}\\ \lambda = B_{33} - \frac{\left[B_{13}^2+v_0(B_{12}B_{13}-B_{11}B_{23})\right]}{B_11} \end{cases}$

得到内参数后，内参矩阵M也已知。单应矩阵H也已知，因此可继续求得外参数：

r 1 = λ M - 1 h 1, r 2 = λ M - 1 h 2 r 3 = r 1 \times r 2, t = λ M - 1 h 3

$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 | | = | | λ M - 1 h 1 | | = 1

$||r_1|| = ||\lambda M^{-1}h_1|| = 1$
则可得

λ = 1 M - 1 h 1

$\lambda = \frac{1}{M^{-1}h_{1}}$

实际情况下，数据中是存在噪音的，所以计算得到的旋转矩阵R并不一定能满足旋转矩阵的性质。所以通常根据奇异值分解来得到旋转矩阵R。

上述的推导结果是基于理想情况下的解，从理论上证明了张氏标定算法的可行性。但在实际标定过程中，一般使用最大似然估计进行优化。假设我们拍摄了n张标定图片，每张图片里有m个棋盘格角点。三维空间点X在图片上对应的二维像素为x，三维空间点经过相机内参M，外参R，t变换后得到的二维像素为x’，假设噪声是独立同分布的，我们通过最小化x, x’的位置来求解上述最大似然估计问题：