单像空间后方交会程序
#include <iostream>
#include <fstream>
#include <cmath>
#include <string>
#include <iomanip>
using namespace std;
const int n=6;
void inverse (double c[n][n]);
template<typename T1,typename T2>void transpose (T1*mat1,T2*mat2,int a,int b);
template<typename T1,typename T2>void multi(T1*mat1,T2 * mat2,T2 * result,int
a,int b,int c);
template<typename T>void input (T*mat,int a,int b);
template<typename T>void output(T*mat,char*s,int a,int b);
int main()
{
ofstream outFile;
cout.precision(5);
double x0=0.0, y0=0.0; double fk=0.15324; //内方位元素
double m=39689; //估算比例尺
double B[4][5]={0.0},R[3][3],XG[6][1],AT[6][8],ATA[6][6],ATL[6][1];
input (B,4,5); //从文件中读取控制点的影像坐标和地面坐标,存入数组 B
double Xs=0.0, Ys=0.0, Zs=0.0,Q=0.0,W=0.0,K=0.0;
double X,Y,Z,L[8][1],A[8][6];
//确定未知数的出始值
for(int i=0;i<4;i++)
{Xs=Xs+B[i][2];
Ys=Ys+B[i][3];
Zs=Zs+B[i][4];
}
Xs=Xs/4; Ys=Ys/4; Zs=Zs/4+m*fk;
int f=0;
do//迭代计算
{f++;
//组成旋转矩阵
R[0][0]=cos(Q)*cos(K)-sin(Q)*sin(W)*sin(K);
R[0][1]=-cos(Q)*sin(K)-sin(Q)*sin(W)*cos(K);
R[0][2]=-sin(Q)*cos(W);
R[1][0]=cos(W)*sin(K);
R[1][1]=cos(W)*cos(K);
R[1][2]=-sin(W);