单像空间后方交会 C++实现

用C++写的单像空间后方交会，附带有控制点文件。

#include 
#include 
#include 
#include 
#include 
using namespace std;
const int n=6;
void inverse （double c[n][n]）;
templatevoid transpose （T1*mat1T2*mat2int aint b）;
templatevoid multi（T1*mat1T2 * mat2T2 * resultint aint bint c）;
templatevoid input （T*matint aint b）;
templatevoid output（T*matchar*sint aint 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 （B45）;   //从文件中读取控制点的影像坐标和地面坐标，存入数组B
	double Xs=0.0 Ys=0.0 Zs=0.0Q=0.0W=0.0K=0.0; 
	double XYZL[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）;
	 R[2][0]=sin（Q）*cos（K）+cos（Q）*sin（W）*sin（K）;
	 R[2][1]=-sin（Q）*sin（K）+cos（Q）*sin（W）*cos（K）;
	 R[2][2]=cos（Q）*cos（W）;
    //计算系数阵和常数项
	 for（int i=0k=0j=0;i<=3;i++k++j++）
	 {
		X=R[0][0]*（B[i][2]-Xs）+R[1][0]*（B[i][3]-Ys）+R[2][0]*（B[i][4]-Zs）;
		Y=R[0][1]*（B[i][2]-Xs）+R[1][1]*（B[i][3]-Ys）+R[2][1]*（B[i][4]-Zs）;
		Z=R[0][2]*（B[i][2]-Xs）+R[1][2]*（B[i][3]-Ys）+R[2][2]*（B[i][4]-Zs）;
	    L[j][0]=B[i][0]-（x0-fk*X/Z）;
		L[j+1][0]=B[i][1]-（y0-fk*Y/Z）;
		j++;
		A[k][0]=（R[0][0]*fk+R[0][2]*（B[i][0]-x0））/Z;
		A[k][1]=（R[1][0]*fk+R[1][2]*（B[i][0]-x0））/Z;
		A[k][2]=（R[2][0]*fk+R[2][2]*（B[i][0]-x0））/Z;
	A[k][3]=（B[i][1]-y0）*sin（W）-（（B[i][0]-x0）*（（B[i][0]-x0）*cos（K）-（B[i][1]-y0）*sin（K））/fk+fk*cos（K））*cos（W）;
	A[k][4]=-fk*sin（K）-（B[i][0]-x0）*（（B[i][0]-x0）*sin（K）+（B[i][1]-y0）*cos（K））/fk;
		A[k][5]=B[i][1]-y0;
		A[k+1][0]=（R[0][1]*fk+R[0][2]*（B[i][1]-y0））/Z;
		A[k+1][1]=（R[1][1]*fk+R[1][2]*（B[i][1]-y0））/Z;
		A[k+1][2]=（R[2][1]*fk+R[2][2]*（B[i][1]-y0））/Z;
	A[k+1][3]=-（B[i][0]-x0）*sin（W）-（（B[i][1]-y0）*（（B[i][0]-x0）*cos（K）-（B[i][1]-y0）*sin（K））/fk-fk*sin（K））*cos（W）;
	A[k+1][4]=-fk*cos（K）-（B[i][1]-y0）*（（B[i][0]-x0）*sin（K）+（B[i][1]-y0）*cos（K））/fk;
		A[k+1][5]=-（B[i][0]-x0）;
		k++;
	}
	transpose（AAT68）;
	multi（ATAATA686）;
	inverse（ATA）;
	multi（ATLATL681）;
	multi（ATAATLXG661）;
	Xs=Xs+XG[0][0]; Ys=Ys+XG[1][0]; Zs=Zs+XG[2][0];
	Q=Q+XG[3][0];  W=W+XG[4][0];  K=K+XG[5][0];
	}while（XG[3][0]>=6.0/206265.0||XG[4][0]>=6.0/206265.0||XG[5][0]>=6.0/206265.0）;
	cout<<“迭代次数为：“<	//精度评定
	double AXG[8][1]V[8][1]VT[1][8]VTV[1][1]m0D[6][6];
	multi（AXGAXG861）;
    for（ int i=0;i<8;i++）     //计算改正数
		V[i][0]=AXG[i][0]-L[i][0];
    transpose （VVT18）;
    multi（VTVVTV181）;
    m0=VTV[0][0]/2;
	for（ int i=0;i<6;i++）
		for（int j=0;j<6;j++）
			D[i][j]=m0*ATA[i][j];

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        179  2014-11-10 11:10  单像空间后方交会 C++\控制点坐标.txt

     文件       5695  2014-11-10 11:10  单像空间后方交会 C++\源.cpp

     目录          0  2014-11-10 11:14  单像空间后方交会 C++

----------- ---------  ---------- -----  ----

                 5874                    3