反解摄像机的投影矩阵

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

齐次向量

首先我们需要反解摄像机的投影矩阵，这里有一些线性代数和图形学的知识。摄像机的投影是一个三维到二维的投影。一般来说，从三维向量$$\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$$了。