反解摄像机的投影矩阵

最近做实验苦于没有真实数据，以便同实验测量结果对比，计算误差大小。此时有一个想法即是，将实验的场景录制下来，然后从录像中找到对应时刻物体的位置，最终得到真实数据。这时就有一个难题：如何从图像中找到三维的坐标值？显然是不可能的，因为少了一维的数据量嘛。不过，在我们的应用场景下，高度是已知的，所以是二维到二维，完全可以得到。

齐次向量

首先我们需要反解摄像机的投影矩阵，这里有一些线性代数和图形学的知识。摄像机的投影是一个三维到二维的投影。一般来说，从三维向量$\vec{x_{(3)}}$到二维向量$\vec{x_{(2)}}$的投影可以写作$2 \times 3$的矩阵$P$，使$\vec{x_{(2)}}=P\vec{x_{(3)}}$（这里的向量都是纵向量）。不过这种向量和矩阵不能表示摄像机的投影矩阵。其一，它不能表示平移；其二，它不能产生近大远小的效果。要改进的话，就要使用齐次向量。

所谓齐次向量，就是给普通向量加一个单位长度。如果一个二维向量是$\vec{x}=(a,b)^T$，那么对应的一个齐次向量就是$\vec{y}=(a,b,1)^T$。如果一个齐次向量是$\vec{y}=(a,b,u)^T$，那么对应的普通向量就是$\vec{x}=1/u\times(a,b)^T$。一个普通向量有无穷个齐次向量对应，一个齐次向量不一定有对应的普通向量（比如$\vec{y}=(1,2,0)^T$）。

投影矩阵

由于引入了齐次向量，投影矩阵也变成了$3 \times 4$的矩阵，在此不妨写开：

$$P=\left(\begin{array}{lll}p_{11} & p_{12} & p_{13} & p_{14}\\p_{21} & p_{22} & p_{23} & p_{24}\\p_{31} & p_{32} & p_{33} & p_{34}\\\end{array}\right)$$

那么，$\vec{x}'=P\vec{x}$就可以写成：

$$\left(\begin{array}{l}x'\\y'\\u'\\\end{array}\right) = \left(\begin{array}{lll}p_{11} & p_{12} & p_{13} & p_{14}\\p_{21} & p_{22} & p_{23} & p_{24}\\p_{31} & p_{32} & p_{33} & p_{34}\\\end{array}\right)\left(\begin{array}{l}x\\y\\z\\u\\\end{array}\right)$$

因为$u$是齐次坐标的单位向量，所以等式两边不都需要写成$u$。而且，在我们的使用中，所有的$u$都不可能为0。此式可以写成以下形式：

$$u\left(\begin{array}{l}x'\\y'\\1\\\end{array}\right) = \left(\begin{array}{lll}p_{11} & p_{12} & p_{13} & p_{14}\\p_{21} & p_{22} & p_{23} & p_{24}\\p_{31} & p_{32} & p_{33} & p_{34}\\\end{array}\right)\left(\begin{array}{l}x\\y\\z\\1\\\end{array}\right)$$

由此，我们的目的就是解出投影矩阵的每一项$p_{ij}, i=1,2,3, j=1,2,3,4$。

线性方程组

如果我们已知$k$个投影前的点的坐标$\vec{x}_i$和投影后的点的坐标$\vec{x}_i'$，每个坐标代入$\vec{x}'=P\vec{x}$可以得到方程组：

$$\left\{\begin{array}{l}u_ix_i'=p_{11}x_i+p_{12}y_i+p_{13}z_i+p_{14}\\u_iy_i'=p_{21}x_i+p_{22}y_i+p_{23}z_i+p_{24}\\u_i=p_{31}x_i+p_{32}y_i+p_{33}z_i+p_{34}\\\end{array}\right.$$

也就是：

$$\left(\begin{array}{lllllllllllll}x_i & y_i & z_i & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & -x_i'\\0 & 0 & 0 & 0 & x_i & y_i & z_i & 1 & 0 & 0 & 0 & 0 & -y_i'\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & x_i & y_i & z_i & 1 & -1\\\end{array}\right)\left(\begin{array}{l}p_{11}\\p_{12}\\p_{13}\\p_{14}\\p_{21}\\p_{22}\\p_{23}\\p_{24}\\p_{31}\\p_{32}\\p_{33}\\p_{34}\\u_i\\\end{array}\right)=\vec{0}$$

如果把所有方程列出来的话就是这样的：

$$\left(\begin{array}{llllllllllllllll}x_1 & y_1 & z_1 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & -x_1' & 0 & \cdots & 0\\0 & 0 & 0 & 0 & x_1 & y_1 & z_1 & 1 & 0 & 0 & 0 & 0 & -y_1' & 0 & \cdots & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & x_1 & y_1 & z_1 & 1 & -1 & 0 & \cdots & 0\\x_2 & y_2 & z_2 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & -x_2' & \cdots & 0\\0 & 0 & 0 & 0 & x_2 & y_2 & z_2 & 1 & 0 & 0 & 0 & 0 & 0 & -y_2' & \cdots & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & x_2 & y_2 & z_2 & 1 & 0 & -1 & \cdots & 0\\\vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\x_k & y_k & z_k & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \cdots & -x_k'\\0 & 0 & 0 & 0 & x_k & y_k & z_k & 1 & 0 & 0 & 0 & 0 & 0 & 0 & \cdots & -y_k'\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & x_k & y_k & z_k & 1 & 0 & 0 & \cdots & -1\\\end{array}\right)\left(\begin{array}{l}p_{11}\\p_{12}\\p_{13}\\p_{14}\\p_{21}\\p_{22}\\p_{23}\\p_{24}\\p_{31}\\p_{32}\\p_{33}\\p_{34}\\u_1\\u_2\\\vdots\\u_k\\\end{array}\right)=\vec{0}$$

这个方程组共有$12+k$个未知量，$3k$个方程。如果要解出解来，须$3k \geq 12+k$，也就是$k \geq 6$。但是还有一个问题，这个方程组是齐次方程组，也就是说：或者所有未知量都为$0$，或者方程之间相关。这里当然是后者。如果我们分析一下$\vec{x}'=P\vec{x}$这个式子，会发现等式左侧的$u$项可以是任意值，而它又是$P$和确定的$\vec{x}$相乘的结果，所以矩阵$P$带有一个和$u$相当的不确定因素。也就是说只要矩阵$P$中的$11$个元素确定了以后，剩下的那个元素的值就已经确定了。具体到我们的方程组上，就是我们可以把一个未知量设为定值，将齐次方程组变为非齐次方程组。

根据之前的推论，按理说应当把$P$中的一个元素设为定值。但这里我们不这么做，原因是$P$中的元素可能为$0$，如果恰好给了一个零元素非零值，那就解不出来了。这里还是对$u$开刀，设$u_k=1$，那么方程就化为：

$$\left(\begin{array}{llllllllllllllll}x_1 & y_1 & z_1 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & -x_1' & 0 & \cdots & 0\\0 & 0 & 0 & 0 & x_1 & y_1 & z_1 & 1 & 0 & 0 & 0 & 0 & -y_1' & 0 & \cdots & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & x_1 & y_1 & z_1 & 1 & -1 & 0 & \cdots & 0\\x_2 & y_2 & z_2 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & -x_2' & \cdots & 0\\0 & 0 & 0 & 0 & x_2 & y_2 & z_2 & 1 & 0 & 0 & 0 & 0 & 0 & -y_2' & \cdots & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & x_2 & y_2 & z_2 & 1 & 0 & -1 & \cdots & 0\\\vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\x_k & y_k & z_k & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \cdots & 0\\0 & 0 & 0 & 0 & x_k & y_k & z_k & 1 & 0 & 0 & 0 & 0 & 0 & 0 & \cdots & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & x_k & y_k & z_k & 1 & 0 & 0 & \cdots & 0\\\end{array}\right)\left(\begin{array}{l}p_{11}\\p_{12}\\p_{13}\\p_{14}\\p_{21}\\p_{22}\\p_{23}\\p_{24}\\p_{31}\\p_{32}\\p_{33}\\p_{34}\\u_1\\u_2\\\vdots\\u_{k-1}\\\end{array}\right)=\left(\begin{array}{l}0\\\vdots\\0\\x_k'\\y_k'\\1\\\end{array}\right)$$

简化记为：

$$A\vec{p}=\vec{b}$$

最小二乘法

上述的方程由于方程个数大于未知量个数，所以不能直接解，而是用最小二乘法求出误差最小的解。对于使用matlab或其他数学库的人来说，本节内容可以跳过了，因为直接调用库函数就好了。

对于一个解$\vec{p_0}$，误差为：

$$\vec{err} = A\vec{p_0} - \vec{b}$$

最小二乘法的目标是最小化误差的平方，也就是：

$$\begin{array}{rl} \vec{err}^2 = & \vec{err}^T\vec{err}\\= & (A\vec{p} - \vec{b})^T(A\vec{p} - \vec{b})\\= & (\vec{p}^TA^T - \vec{b}^T)(A\vec{p} - \vec{b})\\= & \vec{p}^TA^TA\vec{p} - \vec{b}^TA\vec{p} - \vec{p}^TA^T\vec{b} + \vec{b}^T\vec{b}\\= & \vec{p}^TA^TA\vec{p} - 2\vec{b}^TA\vec{p} + \vec{b}^T\vec{b}\\\end{array}$$

设：

$$f(\vec{p})=\vec{err}^2$$

则$f(\vec{p})$是一个多元二次函数，每一元都是一抛物线，且开口都朝上，只要找到导数为$0$的点即是$f(\vec{p})$最小的点。

$$\frac{\partial f(\vec{p})}{\partial \vec{p}} = 2A^TA\vec{p} - 2A^T\vec{b} = 0$$

这是一个简单的非齐次方程组，用高斯消元法解之即可。

总结

在求出$\vec{p}$之后，就可从中取出$p_{ij}$，组成投影矩阵$P$了。