利用最小二乘法求解相机投影矩阵
投影矩阵实验原理
1、相机成像几何模型的建立
为了得到三维空间物体表面某点的几何位置与其所在二维平面图像中对应点之间的相关关系，需要建立相机成像的几何模型。
为了建立几何模型，首先需要构建几个重要的坐标系：
- 世界坐标系（World Coordinate）：在环境中建立的三维坐标系，用来表示摄像机和被摄物体的位置。
- 相机坐标系（Camera Coordinate）：一个三维直角坐标系，原点位于镜头光心处，x、y轴分别与相机成像图片面的两边平行，z轴为镜头光轴，与像平面垂直。
- 像素坐标系：一个二维直角坐标系，反映了相机CCD/CMOS芯片中像素的排列情况，原点o位于图像的左上角，u轴、v轴分别与图像的两边平行，坐标轴的单位是像素（整数）。
- 图像坐标系：一个二维直角坐标系，原点是相机光轴与相面的交点，一般位于图像中心，即图像 的中心点，但是由于制造原因，很多情况下会有偏移，坐标轴的单位通常是毫米（ m m mm mm）。x轴、y轴分别与u轴、v轴平行，故图像坐标系和像素坐标系是平移关系。
世界坐标系到相机坐标系的转换
世界坐标系 X w Y w Z w O w X_wY_wZ_wO_w XwYwZwOw与相机坐标系 X c Y c Z c O c X_cY_cZ_cO_c XcYcZcOc位置如图所示：
通过旋转和平移实现世界坐标系到相机坐标系的转换，在齐次坐标系下具体变换公式为：
[ X c Y c Z c 1 ] = [ R T 0 ⃗ 1 ] [ X w Y w Z w 1 ] (1) \left[ \begin{matrix} {{X}_{c}} \\ {{Y}_{_{c}}} \\ \begin{matrix} {{Z}_{_{c}}} \\ 1 \\ \end{matrix} \\ \end{matrix} \right]=\left[ \begin{matrix} R & T \\ \vec{0} & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {{X}_{w}} \\ {{Y}_{w}} \\ \begin{matrix} {{Z}_{w}} \\ 1 \\ \end{matrix} \\ \end{matrix} \right]\tag1 ⎣ ⎡XcYcZc1⎦ ⎤=[R0T1]⎣ ⎡XwYwZw1⎦ ⎤(1)
相机坐标系到图像坐标系的转换
建立三维相机坐标系 X c Y c Z c O c X_cY_cZ_cO_c XcYcZcOc和二维图像坐标系 x o y xoy xoy的几何关系如下图所示，由成像原理有 X c O c Y c X_cO_cY_c XcOcYc平面与 x o y xoy xoy平面平行， o o o点为镜头光心， O c o O_co Oco的方向为 Z c Z_c Zc轴方向。
P c ( X c , Y c , Z c ) P_c(X_c,Y_c,Z_c) Pc(Xc,Yc,Zc)为三维点，其在二维平面上的投影点为 P ( x , y ) P(x,y) P(x,y)，由相似三角形得公式，其中相机焦距 f = O O c f=OO_c f=OOc：
x X c = f Z c , y Y c = f Z c \frac{x}{{{X_c}}} = \frac{f}{{{Z_c}}},\frac{y}{{{Y_c}}} = \frac{f}{{{Z_c}}} Xcx=Zcf,Ycy=Zcf
在齐次坐标系下的转换公式：
Z c [ x y 1 ] = [ f 0 0 0 0 f 0 0 0 0 1 0 ] [ X c Y c Z c 1 ] (2) {Z_c}\left[ {\begin{array}{cc} x\\ y\\ 1 \end{array}} \right] = \left[ {\begin{array}{cc} f&0&0&0\\ 0&f&0&0\\ 0&0&1&0 \end{array}} \right]\left[ {\begin{array}{cc} {{X_c}}\\ {{Y_c}}\\ {{Z_c}}\\ 1 \end{array}} \right] \tag2 Zc⎣ ⎡xy1⎦ ⎤=⎣ ⎡f000f0001000⎦ ⎤⎣ ⎡XcYcZc1⎦ ⎤(2)
图像坐标系到像素坐标系的转换
建立二维图像坐标系 x o y xoy xoy和二维像素坐标系 u O n v uO_nv uOnv的几何关系如下图所示， O O O点为摄像机光轴与成像平面的交点，一般位于图像中心，但是由于制造原因，很多情况下会有偏移； O n O_n On点位于图像左上角：
引入 d x , d y d_x,d_y dx,dy分别表示 x x x和 y y y方向上单位像素的实际长度，单位为 m m / p x mm/px mm/px，则图像坐标系下点 ( x , y ) (x,y) (x,y)变换到像素坐标系下点 ( u , v ) (u,v) (u,v)的公式为：
{ u = x d x + u 0 v = y d y + v 0 \left\{ {\begin{array}{cc} {{\rm{u}} = \frac{x}{{{d_x}}} + {u_0}}\\ {{\rm{v}} = \frac{y}{{{d_y}}} + {v_0}} \end{array}} \right. {u=dxx+u0v=dyy+v0
在齐次坐标系下的转换公式：
[ u v 1 ] = [ 1 d x 0 u 0 0 1 d y v 0 0 0 1 ] [ x y 1 ] (3) \left[ {\begin{array}{cc} u\\ v\\ 1 \end{array}} \right] = \left[ {\begin{array}{cc} {\frac{1}{{dx}}}&0&{{{\rm{u}}_0}}\\ 0&{\frac{1}{{dy}}}&{{{\rm{v}}_0}}\\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{cc} x\\ y\\ 1 \end{array}} \right] \tag3 ⎣ ⎡uv1⎦ ⎤=⎣ ⎡dx1000dy10u0v01⎦ ⎤⎣ ⎡xy1⎦ ⎤(3)
建立相机几何模型
由公式 ( 1 ) ( 2 ) ( 3 ) (1)(2)(3) (1)(2)(3)可建立相机的几何模型，目标的世界坐标 P ( X w , Y w , Z w ) P(X_w,Y_w,Z_w) P(Xw,Yw,Zw)与目标在像素坐标下的位置 p ( u , v ) p(u,v) p(u,v)转换关系式：
Z c [ u v 1 ] = [ 1 d x 0 u 0 0 1 d y v 0 0 0 1 ] [ f 0 0 0 0 f 0 0 0 0 1 0 ] [ R T 0 → 1 ] [ X w Y w Z w 1 ] = [ f x 0 u 0 0 0 f y v 0 0 0 0 1 0 ] [ R T 0 → 1 ] [ X w Y w Z w 1 ] = M 1 M 2 P w = M P w {Z_c}\left[ {\begin{array}{cc} u\\ v\\ 1 \end{array}} \right] = \left[ {\begin{array}{cc} {\frac{1}{{{d_x}}}}&0&{{u_0}}\\ 0&{\frac{1}{{{d_y}}}}&{{v_0}}\\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{cc} f&0&0&0\\ 0&f&0&0\\ 0&0&1&0 \end{array}} \right]\left[ {\begin{array}{cc} R&T\\ {\overrightarrow 0 }&1 \end{array}} \right]\left[ {\begin{array}{cc} {{X_w}}\\ {{Y_w}}\\ {\begin{array}{cc} {{Z_w}}\\ 1 \end{array}} \end{array}} \right] = \left[ {\begin{array}{cc} {{f_x}}&0&{{u_0}}&0\\ 0&{{f_y}}&{{v_0}}&0\\ 0&0&1&0 \end{array}} \right]\left[ {\begin{array}{cc} R&T\\ {\overrightarrow 0 }&1 \end{array}} \right]\left[ {\begin{array}{cc} {{X_w}}\\ {{Y_w}}\\ {\begin{array}{cc} {{Z_w}}\\ 1 \end{array}} \end{array}} \right] = {M_1}{{\rm{M}}_2}{P_w} = M{P_w} Zc⎣ ⎡uv1⎦ ⎤=⎣ ⎡dx1000dy10u0v01⎦ ⎤⎣ ⎡f000f0001000⎦ ⎤[R0T1]⎣ ⎡XwYwZw1⎦ ⎤=⎣ ⎡fx000fy0u0v01000⎦ ⎤[R0T1]⎣ ⎡XwYwZw1⎦ ⎤=M1M2Pw=MPw
其中：
f x = f / d x f_x=f/d_x fx=f/dx、 f y = f / d y f_y=f/d_y fy=f/dy称为像素坐标系 u 、 v u、v u、v轴的尺度因子；
M 1 M_1 M1称为相机的内部参数矩阵，由摄像机内参数 f x 、 f y 、 u 0 、 v 0 f_x、f_y、u_0、v_0 fx、fy、u0、v0决定；
M 2 M_2 M2称为相机的外部参数矩阵，由旋转矩阵 R R R和平移矩阵 T T T决定；
M M M称为相机的投影矩阵。
引入相机畸变参数
真实世界中相机可能存在偏斜情况，引入畸变参数 γ \gamma γ进行修正，投影矩阵 M M M的最终表达式为：
M = [ f x γ u 0 0 0 f y v 0 0 0 0 1 0 ] [ R T 0 → 1 ] = [ f x γ u 0 0 f y v 0 0 0 1 ] [ R T ] M = \left[ {\begin{array}{cc} {{f_x}}&\gamma &{{u_0}}&0\\ 0&{{f_y}}&{{v_0}}&0\\ 0&0&1&0 \end{array}} \right]\left[ {\begin{array}{cc} R&T\\ {\overrightarrow 0 }&1 \end{array}} \right] = \left[ {\begin{array}{cc} {{f_x}}&\gamma &{{u_0}}\\ 0&{{f_y}}&{{v_0}}\\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{cc} R&T \end{array}} \right] M=⎣ ⎡fx00γfy0u0v01000⎦ ⎤[R0T1]=⎣ ⎡fx00γfy0u0v01⎦ ⎤[RT]
相机内参矩阵受5个参数影响，有5个自由度，外参矩阵旋转平移分别可在3个方向进行，有6个自由度，故
相机投影矩阵共有11个自由度。
2、相机投影矩阵求解
由矩阵乘法特性可知，相机投影矩阵 M M M为一个 3 × 4 3×4 3×4的矩阵，共有11个自由度，即需要求解11个未知参数，构建世界坐标系下的三维点坐标 ( X , Y , Z ) (X,Y,Z) (X,Y,Z)与图像平面上的二维点坐标 ( u , v ) (u,v) (u,v)关系如下：
[ u v 1 ] = [ m 00 m 01 m 02 m 03 m 10 m 11 m 12 m 13 m 20 m 21 m 22 1 ] [ X Y Z 1 ] \left[ {\begin{array}{cc} u\\ v\\ 1 \end{array}} \right] = \left[ {\begin{array}{cc} {{m_{00}}}&{{m_{01}}}&{{m_{02}}}&{{m_{03}}}\\ {{m_{10}}}&{{m_{11}}}&{{m_{12}}}&{{m_{13}}}\\ {{m_{20}}}&{{m_{21}}}&{{m_{22}}}&1 \end{array}} \right]\left[ {\begin{array}{cc} X\\ Y\\ Z\\ 1 \end{array}} \right] ⎣ ⎡uv1⎦ ⎤=⎣ ⎡m00m10m20m01m11m21m02m12m22m03m131⎦ ⎤⎣ ⎡XYZ1⎦ ⎤
对于多个对应点对，有：
{ u i = m 00 X i + m 01 Y i + m 02 Z i + m 03 m 20 X i + m 21 Y i + m 22 Z i + 1 v i = m 10 X i + m 11 Y i + m 12 Z i + m 13 m 20 X i + m 21 Y i + m 22 Z i + 1 \left\{ {\begin{array}{cc} {{u_i} = \frac{{{m_{00}}{X_i} + {m_{01}}{Y_i} + {m_{02}}{Z_i} + {m_{03}}}}{{{m_{20}}{X_i} + {m_{21}}{Y_i} + {m_{22}}{Z_i} + 1}}}\\ {{v_i} = \frac{{{m_{10}}{X_i} + {m_{11}}{Y_i} + {m_{12}}{Z_i} + {m_{13}}}}{{{m_{20}}{X_i} + {m_{21}}{Y_i} + {m_{22}}{Z_i} + 1}}} \end{array}} \right. {ui=m20Xi+m21Yi+m22Zi+1m00Xi+m01Yi+m02Zi+m03vi=m20Xi+m21Yi+m22Zi+1m10Xi+m11Yi+m12Zi+m13
{ u i ( m 20 X i + m 21 Y i + m 22 Z i + 1 ) = m 00 X i + m 01 Y i + m 02 Z i + m 03 v i ( m 20 X i + m 21 Y i + m 22 Z i + 1 ) = m 10 X i + m 11 Y i + m 12 Z i + m 13 \left\{ {\begin{array}{cc} {{u_i}\left( {{m_{20}}{X_i} + {m_{21}}{Y_i} + {m_{22}}{Z_i} + 1} \right) = {m_{00}}{X_i} + {m_{01}}{Y_i} + {m_{02}}{Z_i} + {m_{03}}}\\ {{v_i}\left( {{m_{20}}{X_i} + {m_{21}}{Y_i} + {m_{22}}{Z_i} + 1} \right) = {m_{10}}{X_i} + {m_{11}}{Y_i} + {m_{12}}{Z_i} + {m_{13}}} \end{array}} \right. {ui(m20Xi+m21Yi+m22Zi+1)=m00Xi+m01Yi+m02Zi+m03vi(m20Xi+m21Yi+m22Zi+1)=m10Xi+m11Yi+m12Zi+m13
[ X i Y i Z i 1 0 0 0 0 − u i X i − u i Y i − u i Z i 0 0 0 0 X i Y i Z i 1 − v i X i − v i Y i − v i Z i ] [ m 00 m 01 m 02 m 03 m 10 m 11 m 12 m 13 m 20 m 21 m 22 ] = [ u i v i ] \left[ {\begin{array}{cc} {{X_i}}&{{Y_i}}&{{Z_i}}&1&0&0&0&0&{ - {u_i}{X_i}}&{ - {u_i}{Y_i}}&{ - {u_i}{Z_i}}\\ 0&0&0&0&{{X_i}}&{{Y_i}}&{{Z_i}}&1&{ - {v_i}{X_i}}&{ - {v_i}{Y_i}}&{ - {v_i}{Z_i}} \end{array}} \right]\left[ {\begin{array}{cc} {{m_{00}}}\\ {{m_{01}}}\\ {{m_{02}}}\\ {{m_{03}}}\\ {{m_{10}}}\\ {{m_{11}}}\\ {{m_{12}}}\\ {{m_{13}}}\\ {{m_{20}}}\\ {{m_{21}}}\\ {{m_{22}}}\\ \end{array}} \right] = \left[ {\begin{array}{cc} u_i\\ v_i \end{array}} \right] [Xi0Yi0Zi0100Xi0Yi0Zi01−uiXi−viXi−uiYi−viYi−uiZi−viZi]⎣ ⎡m00m01m02m03m10m11m12m13m20m21m22⎦ ⎤=[uivi]
求解11个未知参数，需要联立11个方程，所以至少需要获取6个对应点对的信息才能求解相机投影矩阵。
而对于获取对应点对信息联立的求解方程组，方程组的个数多于未知数的参数，方程为矛盾/超定方程组，无解，此时需要利用最小二乘法来求解此特定问题。
3、用最小二乘法求解矛盾方程组
给定矛盾方程组：
{ a 11 x 1 + a 12 x 2 + ⋯ a 1 m x m = b 1 a 21 x 1 + a 22 x 2 + ⋯ a 2 m x m = b 2 ⋮ a n 1 x 1 + a n 2 x 2 + ⋯ a n m x m = b n \left\{ {\begin{array}{cc} {{a_{11}}{x_1} + {a_{12}}{x_2} + \cdots {a_{1m}}{x_m} = {b_1}}\\ {{a_{21}}{x_1} + {a_{22}}{x_2} + \cdots {a_{2m}}{x_m} = {b_2}}\\ \vdots \\ {{a_{n1}}{x_1} + {a_{n2}}{x_2} + \cdots {a_{nm}}{x_m} = {b_n}} \end{array}} \right. ⎩ ⎨ ⎧a11x1+a12x2+⋯a1mxm=b1a21x1+a22x2+⋯a2mxm=b2⋮an1x1+an2x2+⋯anmxm=bn
即
∑ j = 1 m a i j x j = b i ( i = 1 , 2 , ⋯ , n ; m < n ) (1) \sum\limits_{j = 1}^m {{a_{ij}}{x_j} = {b_i}\left( {i = 1,2, \cdots ,n;m < n} \right)}\tag1 j=1∑maijxj=bi(i=1,2,⋯,n;m<n)(1)
最小二乘法的基本思想：矛盾方程组无精确解，只能寻求某种意义下的近似解，这种近似解并非是对精确解的近似，而是寻求各未知数的一组值，使方程组中各式能近似相等。
-
方程组中每个方程式精确解与近似解的差值记为，方程组共 n n n个方程 m m m个未知量， m < n m
m<n ：
R i = ∑ j = 1 m a i j x j − b i ( i = 1 , 2 , ⋯ , n ) {R_i} = \sum\limits_{j = 1}^m {{a_{ij}}{x_j} - {b_i}} \left( {i = 1,2, \cdots ,n} \right) Ri=j=1∑maijxj−bi(i=1,2,⋯,n) -
按最小二乘法原则，各个方程式的误差平方和为：
Q = ∑ i = 1 n R i 2 = ∑ i = 1 n [ ∑ j = 1 m a i j x j − b i ] 2 Q = \sum\limits_{i = 1}^n {{R_i}^2} = {\sum\limits_{i = 1}^n {\left[ {\sum\limits_{j = 1}^m {{a_{ij}}{x_j} - {b_i}} } \right]} ^2} Q=i=1∑nRi2=i=1∑n[j=1∑maijxj−bi]2
若 x j ( j = 1 , 2 , ⋯ , m ) x_j(j=1,2,\cdots ,m) xj(j=1,2,⋯,m)取值使得误差平方和 Q Q Q达到最小，则称这组值是矛盾方程组的最优近似解。 -
Q Q Q可以看作 m m m个 x j x_j xj自变量的二次函数且连续，故存在一组数 x 1 , x 2 , ⋯ , x m x_1,x_2,\cdots,x_m x1,x2,⋯,xm，使得 Q Q Q达到最小，转化为极值问题，即需满足：
∂ Q / ∂ x k = 0 ( k = 1 , 2 , ⋯ , m ) {\partial Q}/{\partial {x_k}}= 0\left( {k = 1,2, \cdots ,m} \right) ∂Q/∂xk=0(k=1,2,⋯,m)
得：
∂ Q / ∂ x k = ∑ i = 1 n 2 [ ∑ j = 1 m a i j x j − b i ] a i k = 2 ∑ i = 1 n [ ∑ j = 1 m a i j a i k x j − a i k b i ] = 2 ∑ j = 1 m ( ∑ i = 1 n a i j a i k ) x j − 2 ∑ i = 1 n a i k b i = 0 \begin{aligned} {\partial Q}/{\partial {x_k}}&= \sum\limits_{i = 1}^n {2\left[ {\sum\limits_{j = 1}^m {{a_{ij}}{x_j} - {b_i}} } \right]{a_{ik}}} \\ &= 2\sum\limits_{i = 1}^n {\left[ {\sum\limits_{j = 1}^m {{a_{ij}}{a_{ik}}{x_j} - {a_{ik}}{b_i}} } \right]} \\ &= 2\sum\limits_{j = 1}^m {\left( {\sum\limits_{i = 1}^n {{a_{ij}}{a_{ik}}} } \right)} {x_j} - 2\sum\limits_{i = 1}^n {{a_{ik}}} {b_i}\\ &=0 \end{aligned} ∂Q/∂xk=i=1∑n2[j=1∑maijxj−bi]aik=2i=1∑n[j=1∑maijaikxj−aikbi]=2j=1∑m(i=1∑naijaik)xj−2i=1∑naikbi=0
最终得到极值条件：
∑ j = 1 m ( ∑ i = 1 n a i j a i k ) x j = ∑ i = 1 n a i k b i ( k = 1 , 2 , ⋯ , m ) (2) \sum\limits_{j = 1}^m {\left( {\sum\limits_{i = 1}^n {{a_{ij}}{a_{ik}}} } \right)} {x_j} = \sum\limits_{i = 1}^n {{a_{ik}}} {b_i}\left( {k = 1,2, \cdots ,m} \right)\tag2 j=1∑m(i=1∑naijaik)xj=i=1∑naikbi(k=1,2,⋯,m)(2)
( 2 ) (2) (2)式是含有 m m m个未知量， m m m个方程的线性方程组，是对应于矛盾方程组 ( 1 ) (1) (1)的正规方程组，显然 ( 2 ) (2) (2)的解是 ( 1 ) (1) (1)的最优近似解。 -
记
A = [ a 11 a 12 ⋯ a 1 m a 21 a 22 ⋯ a 2 m ⋮ ⋮ ⋮ a n 1 a n 2 ⋯ a n m ] , x → = ( x 1 x 2 ⋯ x m ) T , b → = ( b 1 b 2 ⋯ b n ) T A = \left[ {\begin{array}{cc} {{a_{11}}}&{{a_{12}}}& \cdots &{{a_{1m}}}\\ {{a_{21}}}&{{a_{22}}}& \cdots &{{a_{2m}}}\\ \vdots & \vdots &{}& \vdots \\ {{a_{n1}}}&{{a_{n2}}}& \cdots &{{a_{nm}}} \end{array}} \right], {\overrightarrow x } = {\left( {\begin{array}{cc} {{x_1}}&{{x_2}}& \cdots &{{x_m}} \end{array}} \right)^T}, {\overrightarrow b } = {\left( {\begin{array}{cc} {{b_1}}&{{b_2}}& \cdots &{{b_n}} \end{array}} \right)^T} A=⎣ ⎡a11a21⋮an1a12a22⋮an2⋯⋯⋯a1ma2m⋮anm⎦ ⎤,x=(x1x2⋯xm)T,b=(b1b2⋯bn)T
则 ( 1 ) 式 ⇔ A x → = b → (1)式\Leftrightarrow A{\overrightarrow x } = {\overrightarrow b } (1)式⇔Ax=b -
记
{ ∑ i = 1 n a i j a i k = c k j ( k , j = 1 , 2 , ⋯ , m ) ∑ i = 1 n a i k b i = d k ( k = 1 , 2 , ⋯ , m ) (3) \left\{ {\begin{array}{cc} {\sum\limits_{i = 1}^n {{a_{ij}}{a_{ik}}} = {c_{kj}}\left( {k,j = 1,2, \cdots ,m} \right)}\\ {\sum\limits_{i = 1}^n {{a_{ik}}} {b_i} = {d_k}\left( {k = 1,2, \cdots ,m} \right)} \end{array}} \right.\tag3 ⎩ ⎨ ⎧i=1∑naijaik=ckj(k,j=1,2,⋯,m)i=1∑naikbi=dk(k=1,2,⋯,m)(3)
则 ( 2 ) 式 ⇔ ∑ j = 1 m c i k x j = d i ( k = 1 , 2 , ⋯ , m ) ⇔ c x → = d → (2)式\Leftrightarrow \sum\limits_{j = 1}^m {{c_{ik}}} {x_j} = {d_i}\left( {k = 1,2, \cdots ,m} \right)\Leftrightarrow c{\overrightarrow x } = {\overrightarrow d } (2)式⇔j=1∑mcikxj=di(k=1,2,⋯,m)⇔cx=d记
c = [ c 11 c 12 ⋯ c 1 m c 21 c 22 ⋯ c 2 m ⋮ ⋮ ⋮ c m 1 c m 2 ⋯ c m m ] , d → = ( d 1 d 2 ⋯ d m ) T c = \left[ {\begin{array}{cc} {{c_{11}}}&{{c_{12}}}& \cdots &{{c_{1m}}}\\ {{c_{21}}}&{{c_{22}}}& \cdots &{{c_{2m}}}\\ \vdots & \vdots &{}& \vdots \\ {{c_{m1}}}&{{c_{m2}}}& \cdots &{{c_{mm}}} \end{array}} \right],{\overrightarrow d } = {\left( {\begin{array}{cc} {{d_1}}&{{d_2}}& \cdots &{{d_m}} \end{array}} \right)^T} c=⎣ ⎡c11c21⋮cm1c12c22⋮cm2⋯⋯⋯c1mc2m⋮cmm⎦ ⎤,d=(d1d2⋯dm)T -
由 ( 3 ) (3) (3)式可知 c = A T A , d → = A T b → c = {A^T}A,{\overrightarrow d } = {A^T}{\overrightarrow b } c=ATA,d=ATb
最终得 c x → = d → ⇔ A T A x = A T b → c{\overrightarrow x } = {\overrightarrow d }\Leftrightarrow{A^T}Ax={A^T}{\overrightarrow b} cx=d⇔ATAx=ATb
-
因此最终得到正规方程组解的求解公式： x → = ( A T A ) − 1 A T b → {\overrightarrow x }= {\left( {{A^T}A} \right)^{ - 1}}{A^T}{\overrightarrow b} x=(ATA)−1ATb
投影矩阵实验过程
使用opencv3.4-C++编写实验，实验过程为：
- 从txt文件中读取数据存入vector数组中；
- 根据超定方程的系数关系构造系数矩阵 A A A，关键代码：
//系数矩阵A
//方程个数2*点个数，未知数个数为11
Mat aA = Mat::zeros(2 * count, 11, CV_64F);
for (int i = 0; i < count; i++)
{
aA.at<double>(2 * i, 0) = x[i];
aA.at<double>(2 * i, 1) = y[i];
aA.at<double>(2 * i, 2) = z[i];
aA.at<double>(2 * i, 3) = 1;
aA.at<double>(2 * i, 4) = 0;
aA.at<double>(2 * i, 5) = 0;
aA.at<double>(2 * i, 6) = 0;
aA.at<double>(2 * i, 7) = 0;
aA.at<double>(2 * i, 8) = -au[i] * x[i];
aA.at<double>(2 * i, 9) = -au[i] * y[i];
aA.at<double>(2 * i, 10) = -au[i] * z[i];
aA.at<double>(2 * i + 1, 0) = 0;
aA.at<double>(2 * i + 1, 1) = 0;
aA.at<double>(2 * i + 1, 2) = 0;
aA.at<double>(2 * i + 1, 3) = 0;
aA.at<double>(2 * i + 1, 4) = x[i];
aA.at<double>(2 * i + 1, 5) = y[i];
aA.at<double>(2 * i + 1, 6) = z[i];
aA.at<double>(2 * i + 1, 7) = 1;
aA.at<double>(2 * i + 1, 8) = -av[i] * x[i];
aA.at<double>(2 * i + 1, 9) = -av[i] * y[i];
aA.at<double>(2 * i + 1, 10) = -av[i] * z[i];
}
- 构造列向量 b → {\overrightarrow b} b，关键代码：
Mat ab = Mat::zeros(2 * count, 1, CV_64F);
for (int i = 0; i < count; i++)
{
ab.at<double>(2 * i) = au[i];
ab.at<double>(2 * i + 1) = av[i];
}
- 根据求解公式 x → = ( A T A ) − 1 A T b → {\overrightarrow x }= {\left( {{A^T}A} \right)^{ - 1}}{A^T}{\overrightarrow b} x=(ATA)−1ATb进行计算得到11维列向量，关键代码：
//根据最小二乘法公式计算投影矩阵，得到11维的列向量
m1 = ((aA.t() * aA).inv()) * (aA.t()) * ab;
- 将投影矩阵的最后一个元素 M [ 3 ] [ 4 ] M[3][4] M[3][4]赋值为1得到最终的投影矩阵。
代码
GitHub代码地址