单像空间后方交会模型

前言

单像空间后方交会原理非常简单，稍稍懂一点线性代数和微分学知识就很容易理解。但它的意义很大，所有的基于视觉技术的定位（测量）都是从它扩展来的，没有它就没有后面的复杂算法的演进，以面向更复杂的场景以及需求。这块内容一般来说就是在已知地面上若干点的地面坐标以后，反求该相应摄影光束的外参数，当用以作摄影机的标定时，还可借以同时求出摄影的内参数。具体的说就是一个多元线性模型，根据不同的需求可以基于它进一步构建各种平差模型。

中心投影的构像方程式

由于我们使用的工业相机最多的是基于针孔相机模型，所以首先需要建立中心投影的几何模型，先从较为简单的模型开始谈起：如下图所示，此时像空间坐标系统与物空间坐标系统的轴线相互平行。

按照三角形相似关系得出：
(X−Xs):x=(Y−Ys):y=(Z−Zs):(−f)(X-X_{s}):x=(Y-Y_{s}):y=(Z-Z^{_{s}}):(-f)(X−Xs):x=(Y−Ys):y=(Z−Zs):(−f)

经整理得
x=−fX−XsZ−Zsy=−fY−YsZ−Zs}\left.\begin{matrix} x=-f \frac{ X-X_{s} } { Z-Z_{s} } \\ \\ y=-f \frac{Y-Y_{s}} {Z-Z_{s}} \end{matrix}\right\} x=−fZ−ZsX−Xsy=−fZ−ZsY−Ys⎭⎬⎫

由上述得正直摄影模型可扩展为一般的情况，如下图所示：

为了要确定物象坐标系的关系，只需做一个简单的正交变换即可，对应于线性模型中乘上一个正交矩阵，假设像坐标系x,y,zx,y,zx,y,z，物坐标系u,v,wu,v,wu,v,w，其关系公式可整理为：
u=a1x+a2y−a3fv=b1x+b2y−b3fw=c1x+c2y−c3f}\left.\begin{matrix} u=a_{1}x+a_{2}y-a_{3}f\\ v=b_{1}x+b_{2}y-b_{3}f\\ w=c_{1}x+c_{2}y-c_{3}f \end{matrix}\right\}u=a1x+a2y−a3fv=b1x+b2y−b3fw=c1x+c2y−c3f⎭⎬⎫
可进一步抽象成矩阵表达，设矩阵RRR：
R=[a1a2a3b1b2b3c1c2c3]R=\begin{bmatrix} a_{1} &a_{2} & a_{3}\\ b_{1} &b_{2} & b_{3}\\ c_{1} &c_{2} & c_{3} \end{bmatrix}R=⎣⎡a1b1c1a2b2c2a3b3c3⎦⎤
则得出：
[uvw]=R[xy−f]\begin{bmatrix} u\\ v\\ w \end{bmatrix}=R\begin{bmatrix} x\\ y\\ -f \end{bmatrix}⎣⎡uvw⎦⎤=R⎣⎡xy−f⎦⎤
RRR矩阵是正交矩阵，属于特殊正交群，是一个三维流形，理论上用三个参数即可有效的表达，这样模型就变成了：
[X−XsY−YsZ−Zs]=λ[uvw]=λ⋅R[xy−f]=λ[a1x+a2y−a3fb1x+b2y−b3fc1x+c2y−c3f]\begin{bmatrix} X-X_{s}\\ Y-Y_{s}\\ Z-Z_{s} \end{bmatrix}=\lambda \begin{bmatrix} u\\ v\\ w \end{bmatrix}=\lambda \cdot R\begin{bmatrix} x\\ y\\ -f \end{bmatrix}=\lambda \begin{bmatrix} a_{1}x+a_{2}y-a_{3}f\\ b_{1}x+b_{2}y-b_{3}f\\ c_{1}x+c_{2}y-c_{3}f \end{bmatrix}⎣⎡X−XsY−YsZ−Zs⎦⎤=λ⎣⎡uvw⎦⎤=λ⋅R⎣⎡xy−f⎦⎤=λ⎣⎡a1x+a2y−a3fb1x+b2y−b3fc1x+c2y−c3f⎦⎤

通过整理，便可得到中心投影的构像方程式，它是视觉测量中最主要的一个方程式：

x=−fa1(X−Xs)+b1(Y−Ys)+c1(Z−Zs)a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)y=−fa2(X−Xs)+b2(Y−Ys)+c2(Z−Zs)a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)}\left.\begin{matrix} x=-f\frac{a_{1}(X-X_{s})+b_{1}(Y-Y_{s})+c_{1}(Z-Z_{s})}{a_{3}(X-X_{s})+b_{3}(Y-Y_{s})+c_{3}(Z-Z_{s})}\\ y=-f\frac{a_{2}(X-X_{s})+b_{2}(Y-Y_{s})+c_{2}(Z-Z_{s})}{a_{3}(X-X_{s})+b_{3}(Y-Y_{s})+c_{3}(Z-Z_{s})} \end{matrix}\right\}x=−fa3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)a1(X−Xs)+b1(Y−Ys)+c1(Z−Zs)y=−fa3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)a2(X−Xs)+b2(Y−Ys)+c2(Z−Zs)}

平差模型的建立

如果是六个外参数（XsX_{s}Xs、YsY_{s}Ys、ZsZ_{s}Zs、φ\varphiφ、ω\omegaω、κ\kappaκ）需要拟合，模型至少利用三个已知点：A(XA,YA,ZA)A(X_{A},Y_{A},Z_{A})A(XA,YA,ZA)、B(XB,YB,ZB)B(X_{B},Y_{B},Z_{B})B(XB,YB,ZB)、C(XC,YC,ZC)C(X_{C},Y_{C},Z_{C})C(XC,YC,ZC)，以及对应的图像坐标：a(xa,ya)a(x_{a},y_{a})a(xa,ya)、b(xb,yb)b(x_{b},y_{b})b(xb,yb)、c(xc,yc)c(x_{c},y_{c})c(xc,yc)。如果拟合的参数大于六个，就需要更多个观测点。目前矩阵RRR还是未知的，建立平差模型前首先要根据具体需求确立RRR的模型，一般来说，欧拉角模型要尽量避免了，除非保证三个偏转角都很小。附加正交的条件也不是很好的选择，因为这相当于提前做降维处理了，但相对于直接拟合欧拉角会好很多，在较为追求效率的场合下，也是可以选择。如果对效率要求不是那么高，可以拟合九参数模型，然后再把拟合好的矩阵投影到特殊正交群上去。
然后就是确定要拟合的参数，假如是要同时得到内外参，就是把矩阵参数以及XsX_{s}Xs，YsY_{s}Ys，ZsZ_{s}Zs，fff作为待定参数。把中心投影的构像方程式展成一阶泰勒，也就是对它进行线性化处理。接着就是对其进行移相整理成如下形式：V=AX−lV=AX-lV=AX−l其中，XXX是待定参数，这里假如X=[XsYsZsfa1a2a3b1b2b3c1c2c3]X=\begin{bmatrix} X_{s} &Y_{s} &Z_{s} &f &a_{1} &a_{2} &a_{3} &b_{1} &b_{2} &b_{3} &c_{1} &c_{2} &c_{3} \end{bmatrix}X=[XsYsZsfa1a2a3b1b2b3c1c2c3]
这时AAA就是2×132\times 132×13矩阵：
A=[∂x∂Xs∂x∂Ys∂x∂Zs∂x∂f∂x∂a1∂x∂a2∂x∂a3∂x∂b1∂x∂b2∂x∂b3∂x∂c1∂x∂c2∂x∂c3∂y∂Xs∂y∂Ys∂y∂Zs∂y∂f∂y∂a1∂y∂a2∂y∂a3∂y∂b1∂y∂b2∂y∂b3∂y∂c1∂y∂c2∂y∂c3]A=\begin{bmatrix} \frac{\partial x}{\partial X_{s}}& \frac{\partial x}{\partial Y_{s}} & \frac{\partial x}{\partial Z_{s}} & \frac{\partial x}{\partial f} & \frac{\partial x}{\partial a_{1}} &\frac{\partial x}{\partial a_{2}} &\frac{\partial x}{\partial a_{3}} &\frac{\partial x}{\partial b_{1}} &\frac{\partial x}{\partial b_{2}} &\frac{\partial x}{\partial b_{3}} &\frac{\partial x}{\partial c_{1}} & \frac{\partial x}{\partial c_{2}} &\frac{\partial x}{\partial c_{3}} \\ \frac{\partial y}{\partial X_{s}}& \frac{\partial y}{\partial Y_{s}}& \frac{\partial y}{\partial Z_{s}} &\frac{\partial y}{\partial f} & \frac{\partial y}{\partial a_{1}} & \frac{\partial y}{\partial a_{2}} & \frac{\partial y}{\partial a_{3}} & \frac{\partial y}{\partial b_{1}} & \frac{\partial y}{\partial b_{2}}& \frac{\partial y}{\partial b_{3}} & \frac{\partial y}{\partial c_{1}} & \frac{\partial y}{\partial c_{2}} & \frac{\partial y}{\partial c_{3}} \end{bmatrix}A=[∂Xs∂x∂Xs∂y∂Ys∂x∂Ys∂y∂Zs∂x∂Zs∂y∂f∂x∂f∂y∂a1∂x∂a1∂y∂a2∂x∂a2∂y∂a3∂x∂a3∂y∂b1∂x∂b1∂y∂b2∂x∂b2∂y∂b3∂x∂b3∂y∂c1∂x∂c1∂y∂c2∂x∂c2∂y∂c3∂x∂c3∂y]
这里还涉及到初始化，假设初始化值：Xs0X_{s}^{0}Xs0，Ys0Y_{s}^{0}Ys0，Zs0Z_{s}^{0}Zs0，f0f^{0}f0，a10a_{1}^{0}a10，a20a_{2}^{0}a20，a30a_{3}^{0}a30，b10b_{1}^{0}b10，b20b_{2}^{0}b20，b30b_{3}^{0}b30，c10c_{1}^{0}c10，c20c_{2}^{0}c20，c30c_{3}^{0}c30。这样，lll可表示为：
l=[x+f0a10(X−Xs0)+b10(Y−Ys0)+c10(Z−Zs0)a30(X−Xs0)+b30(Y−Ys0)+c30(Z−Zs0)y+f0a20(X−Xs0)+b20(Y−Ys0)+c20(Z−Zs0)a30(X−Xs0)+b30(Y−Ys0)+c30(Z−Zs0)]Tl=\begin{bmatrix} x+f^{0}\frac{a_{1}^{0}(X-X_{s}^{0})+b_{1}^{0}(Y-Y_{s}^{0})+c_{1}^{0}(Z-Z_{s}^{0})}{a_{3}^{0}(X-X_{s}^{0})+b_{3}^{0}(Y-Y_{s}^{0})+c_{3}^{0}(Z-Z_{s}^{0})}& y+f^{0}\frac{a_{2}^{0}(X-X_{s}^{0})+b_{2}^{0}(Y-Y_{s}^{0})+c_{2}^{0}(Z-Z_{s}^{0})}{a_{3}^{0}(X-X_{s}^{0})+b_{3}^{0}(Y-Y_{s}^{0})+c_{3}^{0}(Z-Z_{s}^{0})} \end{bmatrix}^{T}l=[x+f0a30(X−Xs0)+b30(Y−Ys0)+c30(Z−Zs0)a10(X−Xs0)+b10(Y−Ys0)+c10(Z−Zs0)y+f0a30(X−Xs0)+b30(Y−Ys0)+c30(Z−Zs0)a20(X−Xs0)+b20(Y−Ys0)+c20(Z−Zs0)]T其中，xxx、yyy、XXX、YYY、ZZZ都是已知的，这样就至少需要7个观测点。接下来如何确定矩阵AAA将是主要问题。中心投影的构像方程式是一个分式，经求导运算可得：
∂x∂Xs=a1f+a3xa3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)\frac{\partial x}{\partial X_{s}}=\frac{a_{1}f+a_{3}x}{a_{3}(X-X_{s})+b_{3}(Y-Y_{s})+c_{3}(Z-Z_{s})}∂Xs∂x=a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)a1f+a3x
∂x∂a1=−f(X−Xs)a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)\frac{\partial x}{\partial a_{1}}=\frac{-f(X-X_{s})}{a_{3}(X-X_{s})+b_{3}(Y-Y_{s})+c_{3}(Z-Z_{s})}∂a1∂x=a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)−f(X−Xs)
∂x∂a3=f[a1(X−Xs)+b1(Y−Ys)+c1(Z−Zs)](X−Xs)[a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)]2\frac{\partial x}{\partial a_{3}}=\frac{f\left [a_{1}(X-X_{s}) +b_{1}(Y-Y_{s}) +c_{1}(Z-Z_{s})\right ](X-X_{s})}{\left [ a_{3}(X-X_{s})+b_{3}(Y-Y_{s})+c_{3}(Z-Z_{s}) \right ]^{2}}∂a3∂x=[a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)]2f[a1(X−Xs)+b1(Y−Ys)+c1(Z−Zs)](X−Xs)
∂x∂f=−a1(X−Xs)−b1(Y−Ys)−c1(Z−Zs)a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)\frac{\partial x}{\partial f}=\frac{-a_{1}(X-X_{s}) -b_{1}(Y-Y_{s}) -c_{1}(Z-Z_{s})}{a_{3}(X-X_{s})+b_{3}(Y-Y_{s})+c_{3}(Z-Z_{s})}∂f∂x=a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)−a1(X−Xs)−b1(Y−Ys)−c1(Z−Zs)
⋯⋯\cdots \cdots ⋯⋯

然后就是套用间接平差公式，这部分请参看测量平差之间接平差，文章写的比较详细，直接往接口传入相应的参数即可。下面是具体的代码实现，其中间接平差部分的代码没有给出，请参考上面的那篇博文。


template<class T, int N>
void getMatrixB(T matrixB[], double3 p3s[N], double2 p2s[N], const double f0, const double3 S0, const double9 A0)
{memset(matrixB, 0, sizeof(T)*N * 2 * 13);for (int i = 0; i != N; ++i){matrixB[i * 26 + 0] = (A0[0] * f0 + A0[2] * p2s[i][0])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 1] = (A0[3] * f0 + A0[5] * p2s[i][0])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 2] = (A0[6] * f0 + A0[8] * p2s[i][0])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 3] = -1*(A0[0] * (p3s[i][0] - S0[0]) + A0[3] * (p3s[i][1] - S0[1]) + A0[6] * (p3s[i][2] - S0[3])) / (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 4] = -1*f0*(p3s[i][0]-S0[0])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 5] = -1 * f0*(p3s[i][1] - S0[1])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 6] = -1 * f0*(p3s[i][2] - S0[2])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 7] = 0;matrixB[i * 26 + 8] = 0;matrixB[i * 26 + 9] = 0;matrixB[i * 26 + 10] = f0* (p3s[i][0] - S0[0])*((A0[0] * (p3s[i][0] - S0[0]) + A0[3] * (p3s[i][1] - S0[1]) + A0[6] * (p3s[i][2] - S0[3])) / pow((A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]), 2);matrixB[i * 26 + 11] = f0* (p3s[i][1] - S0[1])*((A0[0] * (p3s[i][0] - S0[0]) + A0[3] * (p3s[i][1] - S0[1]) + A0[6] * (p3s[i][2] - S0[3]))/ pow((A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]), 2);matrixB[i * 26 + 12] = f0* (p3s[i][2] - S0[2])*((A0[0] * (p3s[i][0] - S0[0]) + A0[3] * (p3s[i][1] - S0[1]) + A0[6] * (p3s[i][2] - S0[3]))/ pow((A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]), 2);matrixB[i * 26 + 13] = (A0[1] * f0 + A0[2] * p2s[i][1])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 14] = (A0[4] * f0 + A0[5] * p2s[i][1])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 15] = (A0[7] * f0 + A0[8] * p2s[i][1])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 16] = -1 * (A0[1] * (p3s[i][0] - S0[0]) + A0[4] * (p3s[i][1] - S0[1]) + A0[7] * (p3s[i][2] - S0[3]))/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 17] = 0;matrixB[i * 26 + 18] = 0;matrixB[i * 26 + 19] = 0;matrixB[i * 26 + 20] = -1 * f0*(p3s[i][0] - S0[0])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 21] = -1 * f0*(p3s[i][1] - S0[1])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 22] = -1 * f0*(p3s[i][2] - S0[2])/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));matrixB[i * 26 + 23] = f0* (p3s[i][0] - S0[0])*((A0[1] * (p3s[i][0] - S0[0]) + A0[4] * (p3s[i][1] - S0[1]) + A0[7] * (p3s[i][2] - S0[3]))/ pow((A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]), 2);matrixB[i * 26 + 24] = f0* (p3s[i][1] - S0[1])*((A0[1] * (p3s[i][0] - S0[0]) + A0[4] * (p3s[i][1] - S0[1]) + A0[7] * (p3s[i][2] - S0[3]))/ pow((A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]), 2);matrixB[i * 26 + 25] = f0* (p3s[i][2] - S0[2])*((A0[1] * (p3s[i][0] - S0[0]) + A0[4] * (p3s[i][1] - S0[1]) + A0[7] * (p3s[i][2] - S0[3]))/ pow((A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]), 2);}
}template<class T, int N>
void getL(T l[], double3 p3s[N], double2 p2s[N], const double f0, const double3 S0, const double9 A0)
{memset(l, 0, sizeof(T)*N * 2);for (int i = 0; i != N; ++i){l[i * 2] = p2s[i][0]+f0*(A0[0] * (p3s[i][0] - S0[0]) + A0[3] * (p3s[i][1] - S0[1]) + A0[6] * (p3s[i][2] - S0[3]))/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));l[i * 2 + 1] = p2s[i][1] + f0*(A0[1] * (p3s[i][0] - S0[0]) + A0[4] * (p3s[i][1] - S0[1]) + A0[7] * (p3s[i][2] - S0[3]))/ (A0[2] * (p3s[i][0] - S0[0]) + A0[5] * (p3s[i][1] - S0[1]) + A0[8] * (p3s[i][2] - S0[3]));}}template<class T, int N>
void getDeltaVector(T DeltaV[], double3 p3s[N], double2 p2s[N], const double f0, const double3 S0, const double9 A0)
{T *matrixB = new T[N * 2 * 13];T *l = new T[N * 2];getMatrixB<T, N>(matrixB, p3s, p2s, f0, S0, A0);getL<T, N>(l, p3s, p2s, f0, S0, A0);GetCorrection(DeltaV, matrixB, l, N * 2, 13);delete[] matrixB;delete[] l;
}

最后拟合好的矩阵AAA不保证是正交矩阵，这时候需要把拟合好的矩阵做投影，投到特殊正交群上去，具体做法就是对AAA做QRQRQR分解，把QQQ矩阵赋给AAA就完成了。QRQRQR分解的代码可参考下面：

/// <summary>
/// QR分解
/// </summary>
template<class T>
int Bmaqr( T Myq[],T Myr[],T Mya[],int Mym,int Myn)
{int i,j,k,l,nn,p,jj;T u,alpha,w,t;if (Mym<Myn){return(0);}for (i=0; i<=Mym-1; i++){for (j=0; j<=Mym-1; j++){l=i*Mym+j; Myq[l]=0.0;if (i==j) {Myq[l]=1.0;}}}for (i=0; i<=Mym-1; i++){for (j=0; j<=Myn-1; j++){l=i*Mym+j; Myr[l]=Mya[l];}}for (i=0; i<=Mym-2; i++){for (j=i+1; j<=Myn-1;j++){p=i*Mym+j; l=j*Mym+i;t=Myr[p]; Myr[p]=Myr[l]; Myr[l]=t;}}nn=Myn;if (Mym==Myn) {nn=Mym-1;}for (k=0; k<=nn-1; k++){u=0.0; l=k*Myn+k;for (i=k; i<=Mym-1; i++){w=fabs(Myr[i*Myn+k]);if (w>u) {u=w;}}alpha=0.0;for (i=k; i<=Mym-1; i++){t=Myr[i*Myn+k]/u; alpha=alpha+t*t;}if (Myr[l]>0.0) {u=-u;}alpha=u*sqrt(alpha);if (fabs(alpha)+1.0==1.0){return(0);}u=sqrt(2.0*alpha*(alpha-Myr[l]));if ((u+1.0)!=1.0){Myr[l]=(Myr[l]-alpha)/u;for (i=k+1; i<=Mym-1; i++){p=i*Myn+k; Myr[p]=Myr[p]/u;}for (j=0; j<=Mym-1; j++){t=0.0;for (jj=k; jj<=Mym-1; jj++){t=t+Myr[jj*Myn+k]*Myq[jj*Mym+j];}for (i=k; i<=Mym-1; i++){p=i*Mym+j; Myq[p]=Myq[p]-2.0*t*Myr[i*Myn+k];}}for (j=k+1; j<=Myn-1; j++){t=0.0;for (jj=k; jj<=Mym-1; jj++){t=t+Myr[jj*Myn+k]*Myr[jj*Myn+j];}for (i=k; i<=Mym-1; i++){p=i*Myn+j; Myr[p]=Myr[p]-2.0*t*Myr[i*Myn+k];}}Myr[l]=alpha;for (i=k+1; i<=Mym-1; i++){Myr[i*Myn+k]=0.0;}}}for (i=0; i<=Mym-2; i++){for (j=i+1; j<=Myn-1;j++){p=i*Mym+j; l=j*Mym+i;t=Myr[p]; Myr[p]=Myr[l]; Myr[l]=t;}}return (1);}

当然，对于单像空间后方交会还有其他的拟合方式，比如教科书上一般是拟合欧拉角了，还可以拟合三角函数，或者附加限制条件……以及根据不同的情况对不同的参数进行拟合等等，在本文就不再赘述了。

精度评定

像点坐标量测的中误差m0m_{0}m0按下式计算（nnn为控制点总数）：m0=vTv2n−6m_{0}=\sqrt{\frac{v^{T}v}{2n-6}}m0=2n−6vTv
由平差理论可知，矩阵(ATA)−1(A^{T}A)^{-1}(ATA)−1等于未知数的协因数阵QxQ_{x}Qx，因此由下式计算未知数的中误差：
mi=m0Qiim_{i}=m_{0}\sqrt{Q_{ii}}mi=m0Qii
式中QiiQ_{ii}Qii是QxQ_{x}Qx的主对角线元素。

参考文献：

[1] 摄影测量原理王之卓编著
[2] 摄影测量学（测绘工程专业）王佩军，徐亚明编著

转载请注明出处：https://blog.csdn.net/fourierFeng/article/details/80174613