摄像机矩阵的分解，求解内外参矩阵（Matlab实现）

1. 应用背景

根据3D点与对应的2D点，能够求解得到摄像机矩阵P，$$\mathrm{P}=\mathrm{K}[\mathrm{R}|\mathrm{t}]\text{\hspace{1em}(1)}$$摄像机矩阵由内参矩阵$\mathrm{K}$, 外参矩阵（包括旋转$\mathrm{R}$和平移$\mathrm{t}$）组成，需要利用$P$矩阵来求解具体的三个分量。

2. 实施原理

定义齐次坐标系下：
$\mathbf{X}$——世界坐标系下三维点坐标，
${\mathbf{X}}_{\mathbf{c}\mathbf{a}\mathbf{m}}$——相机坐标系下三维点坐标，
$\mathbf{x}$——像平面坐标系下二维点坐标。
$\mathrm{P}$——$3\mathrm{x}4$齐次摄像机投影矩阵
$\mathrm{C}$——摄像机中心在世界坐标系中的坐标
在齐次坐标下，3D点到2D点的关系为$$\mathbf{x}=\mathrm{P}\mathbf{X}\text{\hspace{1em}(2)}$$根据坐标变换关系有$$\mathbf{x}=\mathrm{K}[\mathbf{I}|0]{\mathbf{X}}_{\mathbf{c}\mathbf{a}\mathbf{m}}\text{\hspace{1em}(3)}$$其中$[\mathbf{I}|0]$表示矩阵分块成一个3x3单位矩阵加上一个零列矢量。
定义非齐次坐标下
$\tilde{\mathbf{X}}$——世界坐标系下三维点坐标，
${\tilde{\mathbf{X}}}_{\mathbf{c}\mathbf{a}\mathbf{m}}$——相机坐标系下三维点坐标，
$\tilde{\mathbf{x}}$——像平面坐标系下二维点坐标。
$\tilde{\mathrm{C}}$——摄像机中心在世界坐标系中的坐标
在非齐次下，世界坐标系下的点$\mathbf{X}$要转换到相机坐标系下，需要将原点通过平移$\tilde{\mathrm{C}}$进行对齐，然后旋转$\mathrm{R}$来转换，该过程可表示为 $${\tilde{\mathbf{X}}}_{\mathbf{c}\mathbf{a}\mathbf{m}}=\mathrm{R}(\tilde{\mathbf{X}}-\tilde{\mathrm{C}})=\mathrm{R}\tilde{\mathbf{X}}+\mathrm{t}\text{\hspace{1em}(4)}$$ 其中，$\mathrm{R}$是3x3的旋转矩阵,平移量$\mathrm{t}=-\mathrm{R}\tilde{\mathrm{C}}$。该方程在齐次坐标下可以写成 $${\mathbf{X}}_{\mathbf{c}\mathbf{a}\mathbf{m}}=\left[\begin{array}{cc}\mathrm{R}& -\mathrm{R}\tilde{\mathrm{C}}\\ 0& 1\end{array}\right]\mathbf{X}\text{\hspace{1em}(5)}$$ 与式(3)结合为$$\mathbf{x}=\mathrm{K}\mathrm{R}[\mathrm{I}|-\tilde{\mathrm{C}}]\mathbf{X}\text{\hspace{1em}(6)}$$结合式(2)和式(6)，$$\mathrm{P}=\mathrm{K}\mathrm{R}[\mathrm{I}|-\tilde{\mathrm{C}}]$$摄像机矩阵分解的目的就是为了得到其中的参数$\mathrm{K}$、$\mathrm{R}$和$\mathrm{t}$。令$\mathrm{P}$左边$3\mathrm{x}3$的子矩阵为$\mathrm{M}$，于是$$\mathrm{P}=\mathrm{M}[\mathrm{I}|{\mathrm{M}}^{-1}{\mathbf{p}}_4]\text{\hspace{1em}(7)}$$其中，${\mathbf{p}}_4$是$\mathrm{P}$的第4列向量，结合上式，$\mathrm{M}=\mathrm{K}\mathrm{R}$，因为内参数矩阵$\mathrm{K}$是一个上三角阵，旋转矩阵$\mathrm{R}$是一个正交阵，因此，$\mathrm{K}$和$\mathrm{R}$可以由$\mathrm{M}$矩阵通过"RQ分解"来获得。而要得到$\mathrm{t}$，需要先计算$\tilde{\mathrm{C}}$。摄像机中心$\mathrm{C}$是使得$\mathrm{P}\mathrm{C}=0$的点，中心点$\mathrm{C}=(\mathrm{x},\mathrm{y},\mathrm{z},\omega {)}^{\mathrm{T}}$数值分析上可以通过SVD求解，代数上可以通过下面方法得到$$\begin{array}{cc}x=\mathrm{d}\mathrm{e}\mathrm{t}([{\mathbf{p}}_2,{\mathbf{p}}_3,{\mathbf{p}}_4])& y=-\mathrm{d}\mathrm{e}\mathrm{t}([{\mathbf{p}}_1,{\mathbf{p}}_3,{\mathbf{p}}_4])\\ z=\mathrm{d}\mathrm{e}\mathrm{t}([{\mathbf{p}}_1,{\mathbf{p}}_2,{\mathbf{p}}_4])& \omega =-\mathrm{d}\mathrm{e}\mathrm{t}([{\mathbf{p}}_1,{\mathbf{p}}_2,{\mathbf{p}}_3])\end{array}\text{\hspace{1em}(8)}$$进而$\tilde{\mathrm{C}}=(\mathrm{x}/\omega ,\mathrm{y}/\omega ,\mathrm{z}/\mathrm{x}/\omega )$由此，就能计算出平移量$\mathrm{t}$。

3. RQ分解

RQ分解就是通过对待分解矩阵乘一系列的矩阵的矩阵后得到一个上三角阵’R’(这里的符号与上节代表的含义不同)，然后求解正交矩阵。具体地来说，现在要分解P矩阵的子矩阵M，策略就是M右乘一系列的Givens矩阵来求解形如上三角矩阵的内参矩阵K，然后求正交的旋转矩阵R。
三维的Givens旋转是绕三个坐标轴中的一个轴进行的旋转，这三个Givens旋转分别是$Q_x$,$Q_y$,$Q_z$
$$Q_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& c& -s\\ 0& s& c\end{array}\right],Q_y=\left[\begin{array}{ccc}c& 0& s\\ 0& 1& 0\\ -s& 0& c\end{array}\right],Q_z=\left[\begin{array}{ccc}c& -s& 0\\ s& c& 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(9)}$$其中，$c=\cos (\theta )$,$s=\sin (\theta )$。进行“RQ分解”分为如下三步：
3.1 乘$Q_x$使$m_{32}$为零，为了使元素$m_{32}$为零，需要使得$c{m}_{32}+s{m}_{33}=0$, 此方程的解是$$\begin{array}{cc}c=-m_{33}/\sqrt{(m_{32}^2+m_{33}^2))}& s=m_{32}/\sqrt{(m_{32}^2+m_{33}^2))}\end{array}\text{\hspace{1em}(10)}$$ 3.2 乘$Q_y$使$m_{31}$为零，为了使元素$m_{31}$为零，需要使得$c{m}_{31}-s{m}_{33}=0$, 此方程的解是$$\begin{array}{cc}c=m_{33}/\sqrt{(m_{31}^2+m_{33}^2))}& s=m_{31}/\sqrt{(m_{31}^2+m_{33}^2))}\end{array}\text{\hspace{1em}(11)}$$ 3.3 乘$Q_z$使$m_{21}$为零，为了使元素$m_{21}$为零，需要使得$c{m}_{21}+s{m}_{22}=0$, 此方程的解是$$\begin{array}{cc}c=-m_{22}/\sqrt{(m_{21}^2+m_{22}^2))}& s=m_{21}/\sqrt{(m_{21}^2+m_{22}^2))}\end{array}\text{\hspace{1em}(12)}$$由上述三步，将$\mathrm{M}$矩阵的$m_{21},m_{31},m_{32}$元素转换为0，由此得到一个上三角阵$$\mathrm{K}=\mathrm{M}{\mathrm{Q}}_{\mathrm{x}}{\mathrm{Q}}_{\mathrm{y}}{\mathrm{Q}}_{\mathrm{z}}\text{\hspace{1em}(13)}$$因此，$\mathrm{M}=\mathrm{K}{\mathrm{Q}}_{\mathrm{z}}^{\mathrm{T}}{\mathrm{Q}}_{\mathrm{y}}^{\mathrm{T}}{\mathrm{Q}}_{\mathrm{x}}^{\mathrm{T}}$，那么旋转矩阵$$\mathrm{R}={\mathrm{Q}}_{\mathrm{z}}^{\mathrm{T}}{\mathrm{Q}}_{\mathrm{y}}^{\mathrm{T}}{\mathrm{Q}}_{\mathrm{x}}^{\mathrm{T}}\text{\hspace{1em}(14)}$$进一步的平移向量为$$\mathrm{t}=-\mathrm{R}\tilde{\mathrm{C}}\text{\hspace{1em}(15)}$$

4. Matlab实现

%摄像机矩阵分解
clc;clear;close all
p1 = [3.53553e2; -1.03528e2; 7.07107e-1];
p2 = [3.39645e2; 2.33212e1; -3.53553e-1];
p3 = [2.77744e2; 4.59607e2; 6.12372e-1];
p4 = [-1.44946e6; -6.32525e5 ;-9.18559e2];
P = [p1,p2,p3,p4];

x = det([p2,p3,p4]); 
y = -det([p1,p3,p4]);
z = det([p1,p2,p4]);
w = -det([p1,p2,p3]);
C = [x/w;y/w;z/w];
M = P(1:3,1:3);
if det(M) ~= 0
    %乘Qx，使M(3,2)为0
    c = -M(3,3)/sqrt(M(3,2)*M(3,2)+M(3,3)*M(3,3));
    s = M(3,2)/sqrt(M(3,2)*M(3,2)+M(3,3)*M(3,3));
    Qx = [1,0,0; 0,c,-s; 0,s,c];
    M = M*Qx
    %乘Qy，使M(3,1)为0,并且不改变M(3,2)
    c = M(3,3)/sqrt(M(3,1)*M(3,1)+M(3,3)*M(3,3));
    s = M(3,1)/sqrt(M(3,1)*M(3,1)+M(3,3)*M(3,3));
    Qy = [c,0,s; 0,1,0; -s,0,c];
    M = M*Qy
    %乘Qz，使M(2,1)为0,并且不改变M(3,2),M(3,1)
    c = -M(2,2)/sqrt(M(2,1)*M(2,1)+M(2,2)*M(2,2));
    s = M(2,1)/sqrt(M(2,1)*M(2,1)+M(2,2)*M(2,2));
    Qz = [c,-s,0; s,c,0; 0,0,1];
    M = M*Qz
    %上三角阵R=M*Qx*Qy*Qz,正交阵Q=Qz^T*Qy^T*Qx^T
    K = M
    R = Qz'*Qy'*Qx'
end
t = -R*C