利用立体像对两张像片的内方位元素、同名像点坐标和像对的相对方位元素(或外方位元素)解算模型点坐标(或地面点坐标)的工作，称为空间前方交会。在摄影测量中主要有两种：
1.利用立体像对两张像片的相对方位元素，计算模型点的三位坐标；
2.利用立体像对两张像片的外方位元素，计算地面点的地面坐标。
要确定像点与其对应的模型点(地面点)的数学表达式，如上图所示，D-XYZ为地面摄影测量坐标系，S~1~ - U~1~V~1~W~1~及S~2~ - U~2~V~2~W~2~分别为左右像片的像空间辅助坐标系，且两个像空间辅助坐标系的三个轴分别与D-XYZ三轴平行。
设地面点A在D-XYZ坐标系中的坐标为(X,Y,Z),地面点A在S1 - U1V1W1及S2 - U2V2W2的坐标分别为(U1,V1,W1)及(U2,V2,W2)，A点相应的像点坐标a1、a2的像空间坐标为(x1,y1,-f)、(x2,y2,-f)，像点的像空间辅助坐标为(u1,v1,w1)、(u2、v2、w2)，则有：
式中的R1,R2为已知的外方位元素计算的左右像片的旋转矩阵。右摄影站点S2在S1 - U1V1W1中的坐标，即摄影基线B的三个分量Bu,Bv,Bw，可由外方位线元素计算：
因左、右像空间辅助坐标系及D-XYZ相互平行，且摄影站点、像点、地面点三点共线，由此可得出：
式中的N1、N2分别称为左、右像点的投影系数。U1、V1、W1为地面点A在S1 - U1V1W1中的坐标,U2、V2、W2为地面点A在S2 - U2V2W2中的坐标，且
最后得出地面点坐标的公式为：
一般地，在计算地面点Y坐标时，应取均值，即
考虑到(2)式，(5)式又可变为：
由上式的一、三式联立求解，得投影系数的计算式为：
//利用像片外方位元素进行前方交会
#include<iostream>
#include<fstream>
#include <iomanip>
#include "opencv2/opencv2.hpp"
using namespace std;
using namespace cv;
int main()
{
//从文件中读取同名像点坐标到数组
double T_Z_B[2][2] = {0.0 };
ifstream infile1;//定义文件流对象
infile1.open("E:\\space_intersection\\1.txt");//打开文档
double *ptr1 = &T_Z_B[0][0];
while (!infile1.eof())
{
infile1 >> *ptr1;//这是把文档里面的数对应放在ptr位置上的数值上
ptr1++;
}
infile1.close();
double x1 = T_Z_B[0][0];
double y1 = T_Z_B[0][1];
double x2 = T_Z_B[1][0];
double y2 = T_Z_B[1][1];
//定义内方位元素
double x0 = -0.00000;//mm
double y0 = 0.00000;//mm
double f = 70.50;//mm
//从文件中读取外方位元素到数组
/*int i, j;*/
double data[2][6] = { 0.0 };
ifstream infile2;//定义文件流对象
infile2.open("E:\\space_intersection\\2.txt");//打开文档
double *ptr2 = &data[0][0];
while (!infile2.eof())
{
infile2 >> *ptr2;//这是把文档里面的数对应放在ptr位置上的数值上
ptr2++;
}
infile2.close();
//左右像片的外方位元素
double Xs1 = data[0][0];
double Ys1 = data[0][1];
double Zs1 = data[0][2];
double phi1 = data[0][3];
double omig1 = data[0][4];
double kappa1 = data[0][5];
double Xs2 = data[1][0];
double Ys2 = data[1][1];
double Zs2 = data[1][2];
double phi2 = data[1][3];
double omig2 = data[1][4];
double kappa2 = data[1][5];
//cout.precision(12);//控制输出的小数点位数
//计算摄影基线的三个分量
double Bx = Xs2 - Xs1;
double By = Ys2 - Ys1;
double Bz = Zs2 - Zs1;
//利用外方位角元素计算左右像片的旋转矩阵R1和R2，用OpenCV矩阵，方便
double a1 = cos(phi1)*cos(kappa1) - sin(phi1)*sin(omig1)*sin(kappa1);
double a2 = -cos(phi1)*sin(kappa1) - sin(phi1)*sin(omig1)*cos(kappa1);
double a3 = -sin(phi1)*cos(omig1);
double b1 = cos(omig1)*sin(kappa1);
double b2 = cos(omig1)*cos(kappa1);
double b3 = -sin(omig1);
double c1 = sin(phi1)*cos(kappa1) + cos(phi1)*sin(omig1)*sin(kappa1);
double c2 = -sin(phi1)*sin(kappa1) + cos(phi1)*sin(omig1)*cos(kappa1);
double c3 = cos(phi1)*cos(omig1);
Mat R1 = Mat::ones(3, 3, CV_64F);
R1.at<double>(0, 0) = a1;
R1.at<double>(0, 1) = a2;
R1.at<double>(0, 2) = a3;
R1.at<double>(1, 0) = b1;
R1.at<double>(1, 1) = b2;
R1.at<double>(1, 2) = b3;
R1.at<double>(2, 0) = c1;
R1.at<double>(2, 1) = c2;
R1.at<double>(2, 2) = c3;
double a11 = cos(phi2)*cos(kappa2) - sin(phi2)*sin(omig2)*sin(kappa2);
double a22 = -cos(phi2)*sin(kappa2) - sin(phi2)*sin(omig2)*cos(kappa2);
double a33 = -sin(phi2)*cos(omig2);
double b11 = cos(omig2)*sin(kappa2);
double b22 = cos(omig2)*cos(kappa2);
double b33 = -sin(omig2);
double c11 = sin(phi2)*cos(kappa2) + cos(phi2)*sin(omig2)*sin(kappa2);
double c22 = -sin(phi2)*sin(kappa2) + cos(phi2)*sin(omig2)*cos(kappa2);
double c33 = cos(phi2)*cos(omig2);
Mat R2 = Mat::ones(3, 3, CV_64F);
R2.at<double>(0, 0) = a11;
R2.at<double>(0, 1) = a22;
R2.at<double>(0, 2) = a33;
R2.at<double>(1, 0) = b11;
R2.at<double>(1, 1) = b22;
R2.at<double>(1, 2) = b33;
R2.at<double>(2, 0) = c11;
R2.at<double>(2, 1) = c22;
R2.at<double>(2, 2) = c33;
//计算同名像点的像空间辅助坐标系(X1,Y1,Z1)与(X2,Y2,Z2)
Mat RR1, RR2;
Mat R3 = Mat::ones(3, 1, CV_64F);
R3.at<double>(0, 0) = x1;
R3.at<double>(1, 0) = y1;
R3.at<double>(2, 0) = -f;
Mat R33 = Mat::ones(3, 1, CV_64F);
R33.at<double>(0, 0) = x2;
R33.at<double>(1, 0) = y2;
R33.at<double>(2, 0) = -f;
RR1 = R1 * R3;
double X1 = RR1.at<double>(0, 0);
double Y1 = RR1.at<double>(1, 0);
double Z1 = RR1.at<double>(2, 0);
RR2 = R2 * R33;
double X2 = RR2.at<double>(0, 0);
double Y2 = RR2.at<double>(1, 0);
double Z2 = RR2.at<double>(2, 0);
//计算投影系数N1,N2
double N1 = (Bx*Z2 - Bz * X2) / (X1*Z2 - X2 * Z1);
double N2 = (Bx*Z1 - Bz * X1) / (X1*Z2 - X2 * Z1);
//计算地面点的左像辅系坐标(deteX,deteY,deteZ)
double deteX = N1 * X1;
double deteY = 0.5*(N1*Y1 + N2 * Y2 + By);
double deteZ = N1 * Z1;
//计算地面点的地面坐标(X,Y,Z)
double X = Xs1 + deteX;
double Y = Ys1 + deteY;
double Z = Zs1 + deteZ;
cout.precision(11);
cout << "计算得出地面点的地面坐标分别是：" << endl
<< "X=" << X<< endl
<< "Y=" << Y << endl
<< "Z=" << Z<< endl;
cout << "You have finished the work !" << endl;
system("pause");
return 0;
}
空间后方交会的定义是利用地面控制点及其在像片上的像点，确定单幅影像外方位元素的方法。如果已知每张像片的6个外方位元素，就能确定被摄物体与航摄像片的关系，因此，如何获取像片的外方位元素，一直是摄影测量工作者所探讨的问题。目前，采用的测定方法有：利用雷达、全球定位系统(GPS)、惯性导航系统(WS)以及星相摄影机来获取像片的外方位元素；也可利用摄影测量空间后方交会，如下图所示，该方法的基本思想是利用至少三个已知地面控制点的坐标A(XA,YA,ZA)、B(XB,YB,ZB),C(XC,YC,ZC)，与其影像上对应的三个像点坐标a(xa,ya)、b(xb,yb)、c(xc,yc)，根据共线方程，反求该像片的外方位元素XS、YS、ZS、
ϕ
\phi
ϕ、
ω
\omega
ω、
κ
\kappa
κ。这种解算方法是以单张像片为基础，亦称单像空间后方交会。
空间后方交会采用的基本关系式为下面的共线条件方程式：
由于共线条件方程是非线性函数模型，为了便于计算，需把非线性函数表达式用泰勒公式展开为线性形式，人们常把这一数学处理过程称为“线性化”。
将共线式线性化并取一次小值项得：
式中,(x)、(y)为函数的近似值，是将外方位元素初始值XS0、YS0、ZS0、
ϕ
\phi
ϕ0、
ω
\omega
ω0、
κ
\kappa
κ0带入共线条件式所取得的数值。dXS、dYS、dZS、d
ϕ
\phi
ϕ、d
ω
\omega
ω、d
κ
\kappa
κ为外方位元素近似值的改正数。那些偏导数是外方位元素改正数的系数。对于每一个控制点，把像点坐标x、y和相应地面点摄影测量坐标X、Y、Z代入(2)式，就能列出两个方程式。若像片内有三个已知地面控制点，就能列出六个方程式，求出六个外方位元素的改正值。由于(2)式中系数仅取泰勒级数展开式的一次项，未知数的近似值改正是粗略的，所以计算必须采用逐渐趋近法，解求过程需要反复趋近，直至改正数小于某一限值为止。
①获取已知数据：
从摄影资料中查取像片比例尺1/m，平均航高，内方位元素x0,y0,f;
从外业测量成果中，获取控制点的地面测量点坐标Xt,Yt,Zt,并转化为地面摄影测量坐标X、Y、Z。
②量测控制点的像点坐标：
将控制点标刺在像片上，利用立体坐标量测仪量测控制点的像框标坐标，并经像点坐标改正，得到像点坐标x、y。
③确定参数初值：
参数的初值即XS0,YS0,ZS0,
ϕ
\phi
ϕ0,
ω
\omega
ω0,
κ
\kappa
κ0。在竖直航空摄影且地面控制点大体对称分布的情况下，可按如下方法确定初值：
④计算旋转矩阵：
利用角元素的近似值计算方向余弦值，组成R矩阵。
⑤计算像点坐标近似值：
利用参数的近似值，按共线方程计算各个控制点对应像点的像平面坐标近似值xi0,yi0(i=1,2,…n)。
⑥组成误差方程式：
一个控制点对应的误差方程为
写成矩阵形式为 Vi = Ai X – Li ,其中系数矩阵A中的元素均为偏导数。
为了计算这些偏导数，引入以下记号：
推导过程就是各种求偏导数，这里不再描述，直接给出A矩阵中的系数结果
对每一个控制点，计算其对应的方程的系数矩阵Ai、常数项Li，然后联立起来，得：
记为
⑦组成法方程式：计算法方程式的系数矩阵和常数项
按最小二乘原理，取权阵为单位阵，则法方程为：
这一步骤需要计算出ATA和ATL。
⑧解求外方位元素：
根据法方程，按下式解求外方位元素改正数，并与相应的近似值求和，得到外方位元素新的近似值。
⑨检查计算是否收敛：
将求得的外方位元素的改正数与规定的限差比较，小于限差则计算终止，否则用新的近似值重复进行第④至第⑧步的计算，直到满足要求为止。
按照上述方法求得的外方位元素，其精度可以通过法方程的系数矩阵的逆矩阵来求得，即
协因数阵Q的对角线Qii就是第i个未知数的权倒数。若单位权中误差为m0，则第i个微指数的中误差为：
当参加空间后方交会的控制点有n个时，单位权中误差可按下式计算：
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include<iostream>
#include <opencv2\opencv.hpp>
#define GCPNUMBER 4 //设置初始值控制点数量
#define N 6
using namespace cv;
int main()
{
int i, j;
int repeatNumber = 0; //迭代次数
int m = 40000; //比例尺
double x0 = 0, y0 = 0, f = 0.15324; //内方位元素
double outerElement[6] = { 0.0 }; //要求的参数:Xs,Ys,Zs,fai,omiga,kaba;
//输入控制点空间坐标和像坐标xy和XYZ
double GCPLocationxy[GCPNUMBER][2] = { //控制点像坐标
{ -0.08615, -0.06899 },
{ -0.05340, 0.08221 },
{ -0.01478, -0.07663 },
{ 0.01046, 0.06443 } };
double GCPLocationXYZ[GCPNUMBER][3] = { //控制点物空间坐标
{ 36589.41, 25273.32, 2195.17 },
{ 37631.08, 31324.51, 728.69 },
{ 39100.97, 24934.98, 2386.50 },
{ 40426.54, 30319.81, 757.31 } };
printf("单像空间后方交会：\n\n");
printf("已知条件：\n\n");
printf(" 比例尺：\n m = %d\n\n", m);
printf(" 内方位元素：\n x0 = %lf y0 = %lf f = %lf\n\n", x0, y0, f);
printf(" 控制点像坐标：\n");
for (i = 0; i < GCPNUMBER; i++)
{
printf(" ");
for (j = 0; j < 2; j++)
{
printf("%lf ", GCPLocationxy[i][j]);
}
printf("\n");
}
printf("\n 控制点物空间坐标：\n");
for (i = 0; i < GCPNUMBER; i++)
{
printf(" ");
for (j = 0; j < 3; j++)
{
printf("%lf ", GCPLocationXYZ[i][j]);
}
printf("\n");
}
printf("\n迭代过程：");
//给外方位元素赋初值
double sumXYZ[3] = { 0.0 };
for (i = 0; i < 3; i++)
{
for (j = 0; j < GCPNUMBER; j++)
{
sumXYZ[i] += GCPLocationXYZ[j][i];
}
for (j = 0; j < 2; j++)
outerElement[i] = sumXYZ[i] / GCPNUMBER; //XY为四个控制点取平均值
outerElement[i] = m*f + sumXYZ[i] / GCPNUMBER; //Z为m*f
}
Mat X(4 * 2,1,CV_64FC1);
//bool GetMatrixInverse(double **src, int n, double des[N][N]); //求逆矩阵声明
do //迭代过程
{
double R[3][3] = { 0.0 }; //旋转矩阵R
R[0][0] = cos(outerElement[3])*cos(outerElement[5]) - sin(outerElement[3])*sin(outerElement[4])*sin(outerElement[5]);
R[0][1] = -cos(outerElement[3])*sin(outerElement[5]) - sin(outerElement[3]) * sin(outerElement[4])*cos(outerElement[5]);
R[0][2] = -sin(outerElement[3])*cos(outerElement[4]);
R[1][0] = cos(outerElement[4])*sin(outerElement[5]);
R[1][1] = cos(outerElement[4])*cos(outerElement[5]);
R[1][2] = -sin(outerElement[4]);
R[2][0] = sin(outerElement[3])*cos(outerElement[5]) + cos(outerElement[3])*sin(outerElement[4])*sin(outerElement[5]);
R[2][1] = -sin(outerElement[3])*sin(outerElement[5]) + cos(outerElement[3])*sin(outerElement[4])*cos(outerElement[5]);
R[2][2] = cos(outerElement[3])*cos(outerElement[4]);
double Xgang[GCPNUMBER];
for (i = 0; i < GCPNUMBER; i++)
Xgang[i] = R[0][0] * (GCPLocationXYZ[i][0] - outerElement[0]) + R[1][0] * (GCPLocationXYZ[i][1] - outerElement[1])
+ R[2][0] * (GCPLocationXYZ[i][2] - outerElement[2]);
double Ygang[GCPNUMBER];
for (i = 0; i < GCPNUMBER; i++)
Ygang[i] = R[0][1] * (GCPLocationXYZ[i][0] - outerElement[0]) + R[1][1] * (GCPLocationXYZ[i][1] - outerElement[1])
+ R[2][1] * (GCPLocationXYZ[i][2] - outerElement[2]);
double Zgang[GCPNUMBER];
for (i = 0; i < GCPNUMBER; i++)
Zgang[i] = R[0][2] * (GCPLocationXYZ[i][0] - outerElement[0]) + R[1][2] * (GCPLocationXYZ[i][1] - outerElement[1])
+ R[2][2] * (GCPLocationXYZ[i][2] - outerElement[2]);
//xy作为初始值，由共线方程式子求得
double approxxy[GCPNUMBER][2] = { 0.0 };
for (i = 0; i < GCPNUMBER; i++)
{
approxxy[i][0] = -f*Xgang[i] / Zgang[i];
approxxy[i][1] = -f*Ygang[i] / Zgang[i];
}
//误差方程式元素A
Mat A(GCPNUMBER * 2, 6, CV_64FC1);
for (i = 0; i < GCPNUMBER; i++)
{
A.at<double>(i * 2, 0) = (R[0][0] * f + R[0][2] * approxxy[i][0]) / Zgang[i];
A.at<double>(i * 2, 1) = (R[1][0] * f + R[1][2] * approxxy[i][0]) / Zgang[i];
A.at<double>(i * 2, 2) = (R[2][0] * f + R[2][2] * approxxy[i][0]) / Zgang[i];
A.at<double>(i * 2, 3) = (approxxy[i][0] * approxxy[i][1])*R[1][0] / f - (f + pow(approxxy[i][0], 2) / f)*R[1][1] - approxxy[i][1] * R[1][2];
A.at<double>(i * 2, 4) = -pow(approxxy[i][0], 2)*sin(outerElement[5]) / f - (approxxy[i][0] * approxxy[i][1]) / f*cos(outerElement[5]) - f*sin(outerElement[5]);
A.at<double>(i * 2, 5) = approxxy[i][1];
A.at<double>(i * 2 + 1, 0) = (R[0][1] * f + R[0][2] * approxxy[i][1]) / Zgang[i];
A.at<double>(i * 2 + 1, 1) = (R[1][1] * f + R[1][2] * approxxy[i][1]) / Zgang[i];
A.at<double>(i * 2 + 1, 2) = (R[2][1] * f + R[2][2] * approxxy[i][1]) / Zgang[i];
A.at<double>(i * 2 + 1, 3) = -(approxxy[i][0] * approxxy[i][1])*R[1][1] / f + (f + pow(approxxy[i][1], 2) / f)*R[1][0] - approxxy[i][0] * R[1][2];
A.at<double>(i * 2 + 1, 4) = -pow(approxxy[i][1], 2)*cos(outerElement[5]) / f - (approxxy[i][0] * approxxy[i][1]) / f*cos(outerElement[5]) - f*sin(outerElement[5]);
A.at<double>(i * 2 + 1, 5) = -approxxy[i][0];
}
std::cout << A << std::endl;
//误差方程式元素L
Mat L(GCPNUMBER * 2, 1, CV_64FC1);
for (i = 0; i < GCPNUMBER * 2; i++)
{
if (i % 2 == 0) //x
L.at<double>(i) = GCPLocationxy[i / 2][0] - approxxy[i / 2][0];
else
L.at<double>(i) = GCPLocationxy[i / 2][1] - approxxy[i / 2][1];
}
X = (A.t() * A).inv() * A.t() * L;
//求得迭代后的改正值及迭代后的外方位元素
printf("\n\n 第%d次迭代改正值：\n ", ++repeatNumber);
for (i = 0; i < 6; i++)
{
printf("%lf ",X.at<double>(i));
outerElement[i] = outerElement[i] + X.at<double>(i);
}
printf("\n 迭代后外方位元素值：\n ");
for (i = 0; i < 6; i++)
{
printf("%lf ", outerElement[i]);
}
} while (X.at<double>(3) >= 2.908882087e-5 || X.at<double>(4) >= 2.908882087e-5 || X.at<double>(5) >= 2.908882087e-5);//各角元素迭代计算至其改正值小于6秒,6s=2.908882087e-5 弧度。
//精度评价
printf("\n\n\n精度评价矩阵：\n");
double outElemAccuracy[6][6] = { 0.0 };
for (j = 0; j < 6; j++)
{
printf(" ");
for (i = 0; i < 6; i++)
{
outElemAccuracy[j][i] = sqrt(fabs(X.at<double>(j) * X.at<double>(i)) / double(2 * GCPNUMBER - 6));
printf("%lf ", outElemAccuracy[j][i]);
}
printf("\n");
}
system("pause");
}