前言
单像空间后方交会原理非常简单，稍稍懂一点线性代数和微分学知识就很容易理解。但它的意义很大，所有的基于视觉技术的定位（测量）都是从它扩展来的，没有它就没有后面的复杂算法的演进，以面向更复杂的场景以及需求。这块内容一般来说就是在已知地面上若干点的地面坐标以后，反求该相应摄影光束的外参数，当用以作摄影机的标定时，还可借以同时求出摄影的内参数。具体的说就是一个多元线性模型，根据不同的需求可以基于它进一步构建各种平差模型。
中心投影的构像方程式
由于我们使用的工业相机最多的是基于针孔相机模型，所以首先需要建立中心投影的几何模型，先从较为简单的模型开始谈起：如下图所示，此时像空间坐标系统与物空间坐标系统的轴线相互平行。
按照三角形相似关系得出：
(
X
−
X
s
)
:
x
=
(
Y
−
Y
s
)
:
y
=
(
Z
−
Z
s
)
:
(
−
f
)
(X-X_{s}):x=(Y-Y_{s}):y=(Z-Z^{_{s}}):(-f)
(X−Xs):x=(Y−Ys):y=(Z−Zs):(−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=−fZ−ZsX−Xsy=−fZ−ZsY−Ys⎭⎬⎫
由上述得正直摄影模型可扩展为一般的情况，如下图所示：
为了要确定物象坐标系的关系，只需做一个简单的正交变换即可，对应于线性模型中乘上一个正交矩阵，假设像坐标系
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+a2y−a3fv=b1x+b2y−b3fw=c1x+c2y−c3f⎭⎬⎫
可进一步抽象成矩阵表达，设矩阵
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⎦⎤=R⎣⎡xy−f⎦⎤
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}
⎣⎡X−XsY−YsZ−Zs⎦⎤=λ⎣⎡uvw⎦⎤=λ⋅R⎣⎡xy−f⎦⎤=λ⎣⎡a1x+a2y−a3fb1x+b2y−b3fc1x+c2y−c3f⎦⎤
通过整理，便可得到中心投影的构像方程式，它是视觉测量中最主要的一个方程式：
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(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)}
平差模型的建立
如果是六个外参数（
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=AX−l其中，
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=[∂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]
这里还涉及到初始化，假设初始化值：
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(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其中，
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})}
∂Xs∂x=a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)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})}
∂a1∂x=a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)−f(X−Xs)
∂
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}}
∂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
=
−
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})}
∂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;
}
最后拟合好的矩阵 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=2n−6vTv
由平差理论可知，矩阵
(
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
扫一扫
8482
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
