张正友标定法问题

前言

张正友标定法太闻名了，历久不衰，20年后仍然以它作为机器视觉的标定基础，在机器视觉这门课的学习过程中，有个别问题，在此予以记录。

靶标精确度问题

问题描述：在标定前，如果靶标的方格子长度事先不已知，能否标定出相机的内外参数？

符号定义

现有靶标上的点使用齐次坐标$\mathbf{M}_i=(x_i, y_i, 1)^\mathrm{T}$表示，相机平面上对应的点使用$\mathbf{m}_i=(u_i,v_i,1)$表示，以上两组点列值的坐标均已知。相机内参的形式固定且已知，参数待定，形式为：

$$\mathbf{A}=\left[ \begin{matrix} \alpha_x&\gamma&u_0\\ 0&\alpha_y&v_0\\ 0&0&1\\ \end{matrix} \right]$$

相机的外参形式不明确，表示为：

$$\mathbf{D}=\left[ \begin{matrix} r_{11}&r_{12}&t_1\\ r_{21}&r_{22}&t_2\\ r_{31}&r_{32}&t_3\\ \end{matrix} \right]$$

注意到由于外参前两列$\mathbf{r}_1,\mathbf{r}_2$是相机图像坐标系从在相机坐标系得到的旋转矩阵前两列，也即图像坐标系两坐标轴在相机坐标系下的单位矢量方向，第三列$\mathbf{t}$是相机图像坐标系从相机坐标系经平移变换得到的平移矢量，即图像坐标系原点在相机坐标系下的矢量位置。因此外参矩阵$\mathbf{D}$不是正交矩阵，仅前两列满足正交性和规范性，第三列不满足，这个性质在后续分解内外参时会用到。为了方便分析讨论，记$\mathbf{H}=\lambda\mathbf{AD}$为相机的转换矩阵，其中$\lambda$是某一常数因子，因靶标的齐次坐标表示而产生。

问题为：在靶标点和图像点二范数意义下最佳逼近，求解转换矩阵$\mathbf{H}$和内外参数矩阵。

求解转换矩阵

方法一：广义逆矩阵法
某一给定的矩阵方程：$\mathbf{F=PXQ}$，其中$\mathbf{X}$是未知数矩阵，只要两个转换矩阵$\mathbf{P,Q}$的广义逆满秩，该方程就一定能有最佳小二解，即最小二乘解：
$\mathbf{\tilde{X}}=\mathbf{P^+FQ^+}=\mathbf{(P^\mathrm{H}P)^{-1}P^\mathrm{H}FQ^\mathrm{H}(QQ^\mathrm{H})^{-1}}$
因此在这里，如果将各靶标点写成以下形式：

$$\mathbf{m}=\left[\begin{matrix} u_1&u_2&\cdots&u_n\\ v_1&v_2&\cdots&v_n\\ 1&1&\cdots&1\\ \end{matrix} \right]$$

图像点写成以下形式：

$$\mathbf{M}=\left[\begin{matrix} x_1&x_2&\cdots&x_n\\ y_1&y_2&\cdots&y_n\\ 1&1&\cdots&1\\ \end{matrix} \right]$$

则观测值和理想值的原始方程$s\mathbf{m}_i=\mathbf{HM}_i$可以扩展写为：

$$\mathbf{m}=\mathbf{HM}$$

这里为了简化问题，将左式的齐次坐标常数和$\mathbf{H}$矩阵内部的齐次化因子$\lambda$合并，得到以上简化方程形式，该方程的最佳小二解是：$\mathbf{H}^*=\mathbf{mM}^\mathrm{H}(\mathbf{MM}^\mathrm{H})^{-1}$。到此，转换矩阵得以求解，这里需要说明的是，转换矩阵的值与靶标测度有关，不同的靶标比例尺将得到不同的转换矩阵$\mathbf{H}$，它们彼此之间相差一个比例系数。

求解内外参矩阵

外参矩阵前两列的正交性提供两组约束条件：

$$\begin{cases} \mathbf{r_1^\mathrm{H}r_2}&=0\\ \mathbf{r_1^\mathrm{H}r_1}&=\mathbf{r_2^\mathrm{H}r_2} \end{cases}$$

因此，对于单个固定靶标，将得到9+2=11个方程。但是有1+5+9=15个未知数，这便是标定过程中需要更改靶标位置重新拍摄图像的原因。在更改靶标过程中，相机位置保持不变（实际上变不变都无所谓），因此相机的内参认为是保持不变的，给定一个拍摄位置，将会产生一个转换矩阵和外参矩阵，即新产生11个方程和9个未知数（内参不变），因此，只需要三个位置的靶标拍摄图像，未知数个数就不再多于方程个数：

靶标个数	未知数个数	方程个数	可解情况
1	15	11	秩亏
2	24	22	秩亏*
3	33	33	刚好满秩
4	42	44	最优小二解存在

说明：特殊情况下如忽略图像坐标系的不垂直因子$\gamma$时可以认为两个靶标可解。==？（有问题？）==

求解内参矩阵

根据内参矩阵的上三角形式，可以算出其逆矩阵形式，如下式：

$$\mathbf{A}^{-1}=\frac{1}{\alpha_x\alpha_y}\left[ \begin{matrix} \alpha_x&-\gamma&\gamma v_0-\alpha_yu_0\\ 0&\alpha_x&-\alpha_xv_0\\ 0&0&\alpha_x\alpha_y \end{matrix} \right]$$

为了使用$\mathbf{D}$前两列正交的性质，需要对方程$\mathbf{H=AD}$变形得到（内参矩阵一定是满秩的）：$\mathbf{A^{-1}H=D}$，设矩阵

$$\mathbf{H}=\left[ \begin{matrix} h_{11}&h_{12}&h_{13}\\ h_{21}&h_{22}&h_{23}\\ h_{31}&h_{32}&h_{33}\\ \end{matrix} \right]$$

由此可知有以下两个约束条件：

$$\begin{cases} [h_{11},\quad h_{21},\quad h_{31}]\mathbf{A^\mathrm{-T}A}\left[\begin{matrix} h_{12}\\ h_{22}\\ h_{32} \end{matrix} \right]=0\\ [h_{11}-h_{12},\quad h_{21}-h_{22},\quad h_{31}-h_{32}]\mathbf{A^\mathrm{-H}A}\left[\begin{matrix} h_{11}-h_{12}\\ h_{21}-h_{22}\\ h_{31}-h_{32} \end{matrix} \right]=0 \end{cases}$$

由此式可以计算得到参考资料[1]所示的$\mathbf{B}$矩阵，此处略去，该矩阵是一个对称阵，不妨记为：

$$\mathbf{B}=\left[ \begin{matrix} b_{11}&b_{12}&b_{13}\\ b_{21}&b_{22}&b_{23}\\ b_{31}&b_{32}&b_{33}\\ \end{matrix} \right]$$

可以将上述矩阵约束条件改写为：

$$\begin{aligned} \mathbf{b}&=[b_{11},b_{12},b_{22},b_{13},b_{23},b_{33}]^\mathrm{H}\\ \mathbf{v}_{ij}&=[h_{1i}h_{1j},h_{1i}h_{2j}+h_{1j}h_{2i},h_{2i}h_{2j},h_{1i}h_{3j}+h_{1j}h_{3i},h_{2i}h_{3j}+h_{3j}h_{2i},h_{3i}h_{3j}]^\mathrm{H}\\ \left[\begin{matrix} \mathbf{v}_{12}^\mathrm{H}\\ \mathbf{v}_{11}^\mathrm{H}-\mathbf{v}_{22}^\mathrm{H} \end{matrix}\right]\mathbf{b}&=0 \end{aligned}$$

不同位置处得到的转换矩阵将对应不同的$\mathbf{v}_{ij}$矢量，可见约束条件中仅仅用到了矩阵$\mathbf{H}$的==前两列==，第三列则完全==没有参与运算==。将这些矢量按行排列得到矩阵$\mathbf{V}$，则$\mathbf{b}$的最佳小二解就是$\mathbf{V^\mathrm{H}V}$最小特征值对应的特征向量，此时解得的$\mathbf{b}$带有一个比例系数，可对最后一个元素$b_{33}$作归一化操作即可将之消去。

注意到矢量$\mathbf{v}_{ij}$是$h_{ij}$的齐二次式，而根据最佳小二解$\mathbf{H}^*=\mathbf{mM}^\mathrm{H}(\mathbf{MM}^\mathrm{H})^{-1}$可知，$h_{ij}$是靶标坐标$(x,y,1)^\mathbf{H}$的齐负一次式。记精确测量结果带有波浪线上标，当靶标因为测度不准引起的倍缩因子为$k$时，即$\mathbf{M}_i=[x_i,y_i,1]^\mathbf{H}=[k\tilde{x}_i,k\tilde{y}_i,1]^\mathbf{H}$，测不准相当于在靶标坐标上乘以一个对角阵，即有：

$$\mathbf{M}=\left[ \begin{matrix} x_i\\ y_i\\ 1 \end{matrix} \right]=\left[ \begin{matrix} k&0&0\\ 0&k&0\\ 0&0&1\\ \end{matrix} \right]\left[ \begin{matrix} \tilde{x}_i\\ \tilde{y}_i\\ 1 \end{matrix} \right]=\mathbf{K}\left[ \begin{matrix} \tilde{x}_i\\ \tilde{y}_i\\ 1 \end{matrix} \right]=\mathbf{K\tilde M}$$

矩阵$\mathbf{K}$可逆，逆矩阵也是对角阵，因此可改写转换矩阵：

$$\begin{aligned} \mathbf{H}&=\mathbf{mM}^\mathrm{H}(\mathbf{MM}^\mathrm{H})^{-1}\\ &=\mathbf{mM}^\mathrm{H}\mathbf{K}^\mathrm{H}(\mathbf{K\tilde M\tilde M}^\mathrm{H}\mathbf{K}^\mathrm{H})^{-1}\\ &=\mathbf{mM}^\mathrm{H}\mathbf{K}^\mathrm{H}\mathbf{K}^{-1}(\mathbf{\tilde M\tilde M}^\mathrm{H})^{-1}\mathbf{K}^\mathrm{-H}\\ &=\mathbf{mM}^\mathrm{H}(\mathbf{\tilde M \tilde M}^\mathrm{H})^{-1}\mathbf{K}^\mathrm{-1}\\ &=\mathbf{\tilde{H}K^{-1}} \end{aligned}$$

代入后式可知：$\mathbf{\tilde{H}}$前两列相比$\mathbf{H}$多乘了一个常数因子$\frac{1}{k}$，而第三列保持与$\mathbf{H}$相同。在求解内参矩阵时，只用到了$\mathbf{\tilde H}$的前两列的正交性和规范性约束条件，多乘的常数因子会使矢量$\mathbf{v}_{ij}$的各个元素均变成已知精确棋盘长度结果对应矢量$\mathbf{\tilde v}_{ij}$的$\frac{1}{k^2}$，即$\mathbf{v}_{ij}=\frac{1}{k^2}\mathbf{\tilde v}_{ij}$，得到的$\mathbf{V}$矩阵也相应变化，这部分变化体现在$\mathbf{B}$矩阵的比例系数上，而该因子将随元素$b_{33}$归一化过程消去。因此不精确的棋盘格也可准确标定相机内部参数。然后，对解得的矩阵$\mathbf{B}$作Cholesky矩阵分解，即将一个对称阵分解为两个相同的上三角阵乘积，即可解得内参矩阵。

求解外参矩阵

解得内参数矩阵后，对方程$\mathbf{H=AD}\implies\mathbf{D=A^{-1}H}$即可解得外参数矩阵。这里，外参数矩阵使用到了$\mathbf{H}$矩阵的所有元素，因此其值受靶标精度决定，不精确的靶标也会得到不精确的外参数矩阵，这说明使用张正友标定法不能较为精确地标定外参矩阵（虽然这也没有什么必要）。

写在后面

这个推导过程重点在求解内参矩阵上，关键是理解引入的倍缩矩阵$\mathbf{K}$比例倍缩改变了前两列，而解内参矩阵时只用到了前两列，正好形成了齐次关系，所以不精确测量产生的倍缩可以被消除。
如果棋盘不是均匀的，或者镜头产生了畸变，则前两列的齐次关系将被破坏，使用张正友标定法也得不到准确的内参矩阵了。

参考资料

[1] 张广军，机器视觉，科学出版社