如何通俗理解相机标定公式推导与Python编程实践教程

今日语：学习一个算法重要的就是弄清楚这个算法要解决怎样的问题，它的已知量（输入）是什么，待求解的未知量（输出）是什么。——@司南牧

相机标定（Calibration）做了什么事？为何很多地方都需要相机标定？

相机标定主要是为了通过对某个特殊形状的物体拍照从而找到照片中的像素点坐标与现实世界中的三维坐标之间的变换关系。这个变换关系通常是一个矩阵。总结一下“相机标定是想求如何将将现实世界的三维点坐标变换到像素点坐标的那个矩阵”。学习一个算法重要的就是弄清楚这个算法要解决怎样的问题，它的已知量（输入）是什么，待求解的未知量（输出）是什么。现在我们已经知道相机标定要解决一个怎样的问题了。那么一般的相机标定算法它的已知量（输入）是什么，待求解的未知量（输出）是什么？我举个例子（注意这里涉及的坐标都是用齐次坐标来表示）。一般相机标定算法是已知某个像素对应的真实物体点在世界坐标系中的三维坐标$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$（虽然这个很难得到），已知像素点在照片中的二维坐标$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$（这个非常容易）。待求解的未知量为将前面提到的三维坐标变成二维坐标的矩阵$\mathbf{P}$，这个矩阵叫做相机矩阵。即$c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\mathbf{P}\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$。其中$c$是常数。因为$\mathbf{P}\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$的结果一般是这种形式$\left[\begin{array}{c}a\\ b\\ c\end{array}\right]$为了让这个齐次坐标变成我们已知的像素坐标$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$这种形式即坐标中的第3位变成1，大家一般会将$c$提取出来。

总结：
相机标定算法中，
已知：像素点坐标$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$，像素点对应真实世界中的那个点在世界坐标系下的三维齐次坐标$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$。
待求解的未知量：将前面提到的三维齐次坐标$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$变成二维坐标$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$的矩阵$\mathbf{P}$。这个矩阵叫做相机矩阵。
线性等式约束：$c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\mathbf{P}\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$。
这里我只写了一个点对应的方程。解这种线性方程组已经有很多编程工具可以解的，我们后面再讲怎么解。我想你应该已经从直观上以及从数学上已经了解了相机标定的具体内涵。

但是为何我们需要相机标定？这个似乎没有完全详细的解释。因为前面只提到了相机矩阵$\mathbf{P}$可以将世界坐标系中的三维坐标变换到照片中的二维坐标。但是我们看到好像很多文章提到了相机标定也可以正畸，求相机内参数矩阵（intrinsic matrix）。这就不得不分析下相机标定所计算出的相机矩阵$\mathbf{P}$到底是什么？由什么组成的？

接下来我们分析下相机矩阵到底是一个什么东西？

相机矩阵$\mathbf{P}$到底是由什么组成的？

这个还是得回到相机标定解决的问题中来。相机标定解决的问题就是计算出世界坐标系中的坐标变换到像素坐标的矩阵$P$。所以这个里面有两个变换。一个是世界坐标系三维坐标变到相机坐标系中的三维坐标，另一个是相机坐标系中的三维坐标变到照片中的二维坐标。所以相机矩阵$\mathbf{P}$就是这两个变换中的变换矩阵相乘而得到。
下面这段文字需要你对相机成像原理有一点点了解（需要知道相机内参数矩阵是什么，可以参考这篇针孔相机模型教程）。

有过一点坐标变换的知识的同学很容易知道世界坐标系三维齐次坐标$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$变到相机坐标系中的三维齐次坐标$\left[\begin{array}{c}e\\ f\\ g\\ 1\end{array}\right]$的那个变换可以通过乘个矩阵来实现。这个矩阵是齐次变换矩阵一般写作$\mathbf{T}$，它是一个$4\times 4$的矩阵。大家一般把刚刚提到的$\mathbf{T}$这个矩阵叫做外参数矩阵(Extrinsic parameter matrix)。现在$\mathbf{T}$可以将一个世界坐标系中的三维齐次坐标变换到相机坐标系中的三维齐次坐标。然后由于接下来我们需要将三维齐次坐标变成二维齐次坐标，三维齐次坐标是有四个维度，而二维齐次坐标是三个维度，所以需要减少一个维度。因此我们需要用一个矩阵$\mathbf{M}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]$将这个齐次坐标$\left[\begin{array}{c}e\\ f\\ g\\ 1\end{array}\right]$变成相机坐标系中普通的三维坐标$\left[\begin{array}{c}e\\ f\\ g\end{array}\right]$。最后再将相机坐标系中的三维坐标$\left[\begin{array}{c}e\\ f\\ g\end{array}\right]$变到照片中的二维齐次坐标$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$的那个矩阵就是相机内参数矩阵（Intrinsic parameter matrix）一般用符号$\mathbf{K}$表示，它是一个$3\times 3$的矩阵。如果你不了解相机内参数矩阵可以看看针孔相机模型教程。从前面的描述也可以看到相机矩阵$\mathbf{P}$里面是包含了内参数矩阵，也就是说包含了相机的一些参数可以帮助我们正畸。现在就基本从数学上和直观上解释通了相机标定的作用了和原理了。

我将前面这段话翻译成一条公式，大家对比着多看前面这段话和下面这个公式几次就懂了。
$\mathbf{K}\mathbf{M}\mathbf{T}\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$

也就是说$\mathbf{P}=\mathbf{K}\mathbf{M}\mathbf{T}$，前面提到了$\mathbf{K}$是$3\times 3$的矩阵，$\mathbf{M}$是$3\times 4$的矩阵，$\mathbf{T}$是$4\times 4$的矩阵，所以可以推出$\mathbf{P}$是$3\times 4$的矩阵（涉及到矩阵的公式和编程了解各个矩阵的维度很关键）。所以$\mathbf{P}$里面含有一共有$3\ast 4=12$个待求解变量。为何需要提到有12个待求解变量？因为在高中我们学过，多少个等式约束就可以求解多少个变量。在照片中一个二维点对应两个等式约束，所以需要知道照片中至少6个特征点的二维坐标以及它们在世界坐标系中的三维坐标才能凑够12个等式约束才能求出$\mathbf{P}$。

如何求解相机矩阵$\mathbf{P}$

一般是用DLT（direct linear transform）算法求解。从这个名字就看得出它就是直接求解前面提到的那个线性方程。下面就详细讲解如何直接解线性方程的方式来求解$\mathbf{P}$。
$c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\mathbf{P}\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$
由于c这个常数无关紧要所以我们可以用一个小技巧消除它,注意$\times$是指叉乘，（下面这个变换利用了一个很常用的叉乘的性质：两个共线的向量叉乘结果等于0）。
$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\times \mathbf{P}\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=0$

而叉乘又可以转换成矩阵相乘的形式（不懂可以百度叉乘矩阵）
$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\times \mathbf{P}\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{ccc}0& -1& v\\ 1& 0& -u\\ -v& u& 0\end{array}\right]\mathbf{P}\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=0$
注意：$\mathbf{P}$是待求解量，其他都是已知量。

我们平常习惯性求解线性方程是酱紫的$\mathbf{A}\mathbf{P}=0$，所以我们也希望能把上面的那个矩阵相乘的等式换成$\mathbf{A}\mathbf{P}=0$这种形式。我们只需要将上面那几个矩阵相乘的等式展开重新写一个矩阵相乘的等式即可。我们将$\mathbf{P}$写成三个行向量的形式，下面这个式子中$P_1,P_2,P_3$是$\mathbf{P}$的行。我们记$\mathbf{X}=\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$
$\left[\begin{array}{ccc}0& -1& v\\ 1& 0& -u\\ -v& u& 0\end{array}\right]\left[\begin{array}{c}{P}_1\\ P_2\\ P_3\end{array}\right]\mathbf{X}=\left[\begin{array}{ccc}0& -1& v\\ 1& 0& -u\\ -v& u& 0\end{array}\right]\left[\begin{array}{c}{P}_1^T\mathbf{X}\\ P_2^T\mathbf{X}\\ P_3^T\mathbf{X}\end{array}\right]=\left[\begin{array}{ccc}0\ast P_1^T\mathbf{X}& -1\ast P_2^T\mathbf{X}& v\ast P_3^T\mathbf{X}\\ 1\ast P_1^T\mathbf{X}& 0\ast P_2^T\mathbf{X}& -u\ast P_3^T\mathbf{X}\\ -v\ast P_1^T\mathbf{X}& u\ast P_2^T\mathbf{X}& 0\ast P_3^T\mathbf{X}\end{array}\right]=0$
为了把$P_i$从左侧变到右侧，我们对等式两边求转置得到下式：
$\left[\begin{array}{ccc}0\ast {\mathbf{X}}^T{P}_1& 1\ast {\mathbf{X}}^T{P}_2& -v\ast {\mathbf{X}}^T{P}_3\\ -1\ast {\mathbf{X}}^T{P}_1& 0\ast {\mathbf{X}}^T{P}_2& u\ast {\mathbf{X}}^T{P}_3\\ v\ast {\mathbf{X}}^T{P}_1& -u\ast {\mathbf{X}}^T{P}_2& 0\ast {\mathbf{X}}^T{P}_3\end{array}\right]=\left[\begin{array}{ccc}0\ast {\mathbf{X}}^T& 1\ast {\mathbf{X}}^T& -v\ast {\mathbf{X}}^T\\ -1\ast {\mathbf{X}}^T& 0\ast {\mathbf{X}}^T& u\ast {\mathbf{X}}^T\\ v\ast {\mathbf{X}}^T& -u\ast {\mathbf{X}}^T& 0\ast {\mathbf{X}}^T\end{array}\right]\left[\begin{array}{c}{P}_1\\ P_2\\ P_3\end{array}\right]=0$

现在我们已经将等式变成左侧是已知矩阵，右侧是待求解的矩阵的形式了。这种形式可以有很多方法进行求解。比如SVD求线性方程。

参考文献：
[1] https://ww2.mathworks.cn/help/vision/ug/camera-calibration.html
[2] https://blog.csdn.net/varyshare/article/details/92096277
[3] https://github.com/varyshare/easy_slam_tutorial