单像空间后方交会模型

前言

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

中心投影的构像方程式

由于我们使用的工业相机最多的是基于针孔相机模型,所以首先需要建立中心投影的几何模型,先从较为简单的模型开始谈起:如下图所示,此时像空间坐标系统与物空间坐标系统的轴线相互平行。
这里写图片描述
按照三角形相似关系得出:
( X − X s ) : x = ( Y − Y s ) : y = ( Z − Z s ) : ( − f ) (X-X_{s}):x=(Y-Y_{s}):y=(Z-Z^{_{s}}):(-f) (XXs):x=(YYs):y=(ZZs):(f)

经整理得
x = − f X − X s Z − Z s y = − f Y − Y s Z − Z s } \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=fZZsXXsy=fZZsYYs

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

这里写图片描述

为了要确定物象坐标系的关系,只需做一个简单的正交变换即可,对应于线性模型中乘上一个正交矩阵,假设像坐标系 x , y , z x,y,z x,y,z,物坐标系 u , v , w u,v,w u,v,w,其关系公式可整理为:
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 } \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+a2ya3fv=b1x+b2yb3fw=c1x+c2yc3f
可进一步抽象成矩阵表达,设矩阵 R R R
R = [ a 1 a 2 a 3 b 1 b 2 b 3 c 1 c 2 c 3 ] 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
则得出:
[ u v w ] = R [ x y − f ] \begin{bmatrix} u\\ v\\ w \end{bmatrix}=R\begin{bmatrix} x\\ y\\ -f \end{bmatrix} uvw=Rxyf
R R R矩阵是正交矩阵,属于特殊正交群,是一个三维流形,理论上用三个参数即可有效的表达,这样模型就变成了:
[ X − X s Y − Y s Z − Z s ] = λ [ u v w ] = λ ⋅ R [ x y − f ] = λ [ 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 ] \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} XXsYYsZZs=λuvw=λRxyf=λa1x+a2ya3fb1x+b2yb3fc1x+c2yc3f

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

x = − f 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 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 ) } \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(XXs)+b3(YYs)+c3(ZZs)a1(XXs)+b1(YYs)+c1(ZZs)y=fa3(XXs)+b3(YYs)+c3(ZZs)a2(XXs)+b2(YYs)+c2(ZZs)}

平差模型的建立

如果是六个外参数( X s X_{s} Xs Y s Y_{s} Ys Z s Z_{s} Zs φ \varphi φ ω \omega ω κ \kappa κ)需要拟合,模型至少利用三个已知点: A ( X A , Y A , Z A ) A(X_{A},Y_{A},Z_{A}) A(XA,YA,ZA) B ( X B , Y B , Z B ) B(X_{B},Y_{B},Z_{B}) B(XB,YB,ZB) C ( X C , Y C , Z C ) C(X_{C},Y_{C},Z_{C}) C(XC,YC,ZC),以及对应的图像坐标: a ( x a , y a ) a(x_{a},y_{a}) a(xa,ya) b ( x b , y b ) b(x_{b},y_{b}) b(xb,yb) c ( x c , y c ) c(x_{c},y_{c}) c(xc,yc)。如果拟合的参数大于六个,就需要更多个观测点。目前矩阵 R R R还是未知的,建立平差模型前首先要根据具体需求确立 R R R的模型,一般来说,欧拉角模型要尽量避免了,除非保证三个偏转角都很小。附加正交的条件也不是很好的选择,因为这相当于提前做降维处理了,但相对于直接拟合欧拉角会好很多,在较为追求效率的场合下,也是可以选择。如果对效率要求不是那么高,可以拟合九参数模型,然后再把拟合好的矩阵投影到特殊正交群上去。
然后就是确定要拟合的参数,假如是要同时得到内外参,就是把矩阵参数以及 X s X_{s} Xs Y s Y_{s} Ys Z s Z_{s} Zs f f f作为待定参数。把中心投影的构像方程式展成一阶泰勒,也就是对它进行线性化处理。接着就是对其进行移相整理成如下形式: V = A X − l V=AX-l V=AXl其中, X X X是待定参数,这里假如 X = [ 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 ] 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]
这时 A A A就是 2 × 13 2\times 13 2×13矩阵:
A = [ ∂ x ∂ X s ∂ x ∂ Y s ∂ x ∂ Z s ∂ x ∂ f ∂ x ∂ a 1 ∂ x ∂ a 2 ∂ x ∂ a 3 ∂ x ∂ b 1 ∂ x ∂ b 2 ∂ x ∂ b 3 ∂ x ∂ c 1 ∂ x ∂ c 2 ∂ x ∂ c 3 ∂ y ∂ X s ∂ y ∂ Y s ∂ y ∂ Z s ∂ y ∂ f ∂ y ∂ a 1 ∂ y ∂ a 2 ∂ y ∂ a 3 ∂ y ∂ b 1 ∂ y ∂ b 2 ∂ y ∂ b 3 ∂ y ∂ c 1 ∂ y ∂ c 2 ∂ y ∂ c 3 ] 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=[XsxXsyYsxYsyZsxZsyfxfya1xa1ya2xa2ya3xa3yb1xb1yb2xb2yb3xb3yc1xc1yc2xc2yc3xc3y]
这里还涉及到初始化,假设初始化值: X s 0 X_{s}^{0} Xs0 Y s 0 Y_{s}^{0} Ys0 Z s 0 Z_{s}^{0} Zs0 f 0 f^{0} f0 a 1 0 a_{1}^{0} a10 a 2 0 a_{2}^{0} a20 a 3 0 a_{3}^{0} a30 b 1 0 b_{1}^{0} b10 b 2 0 b_{2}^{0} b20 b 3 0 b_{3}^{0} b30 c 1 0 c_{1}^{0} c10 c 2 0 c_{2}^{0} c20 c 3 0 c_{3}^{0} c30。这样, l l l可表示为:
l = [ x + f 0 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 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 ) ] T l=\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(XXs0)+b30(YYs0)+c30(ZZs0)a10(XXs0)+b10(YYs0)+c10(ZZs0)y+f0a30(XXs0)+b30(YYs0)+c30(ZZs0)a20(XXs0)+b20(YYs0)+c20(ZZs0)]T其中, x x x y y y X X X Y Y Y Z Z Z都是已知的,这样就至少需要7个观测点。接下来如何确定矩阵 A A A将是主要问题。中心投影的构像方程式是一个分式,经求导运算可得:
∂ x ∂ X s = a 1 f + a 3 x a 3 ( X − X s ) + b 3 ( Y − Y s ) + c 3 ( Z − Z s ) \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})} Xsx=a3(XXs)+b3(YYs)+c3(ZZs)a1f+a3x
∂ x ∂ a 1 = − f ( X − X s ) a 3 ( X − X s ) + b 3 ( Y − Y s ) + c 3 ( Z − Z s ) \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})} a1x=a3(XXs)+b3(YYs)+c3(ZZs)f(XXs)
∂ x ∂ a 3 = f [ a 1 ( X − X s ) + b 1 ( Y − Y s ) + c 1 ( Z − Z s ) ] ( X − X s ) [ a 3 ( X − X s ) + b 3 ( Y − Y s ) + c 3 ( Z − Z s ) ] 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}} a3x=[a3(XXs)+b3(YYs)+c3(ZZs)]2f[a1(XXs)+b1(YYs)+c1(ZZs)](XXs)
∂ x ∂ f = − 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 ) \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})} fx=a3(XXs)+b3(YYs)+c3(ZZs)a1(XXs)b1(YYs)c1(ZZs)
⋯ ⋯ \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;
}

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

/// <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);

}

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

精度评定

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

参考文献:

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

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

参与评论 您还未登录,请先 登录 后发表或查看评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:Age of Ai 设计师:meimeiellie 返回首页

打赏作者

玉名卍

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值