#include <stdio.h>
#include<iostream>
#include<math.h>
#include <iomanip>
using namespace std;
#define PI 3.1415926535897932
#define N 20
void print(double p[N][N],int m,int n);
//矩阵转置函数
void Transpose (double MOrigin[N][N],double MResult[N][N],int m,int n)
{
int i, j;
for(i=0; i<m; i++)
{
for(j=0; j<n; j++)
MResult[j][i] = MOrigin[i][j];
}
//cout<<"***********转置矩阵输出结果*********"<<endl;
//print(MResult,n,m);
}
//矩阵求逆函数
void Inverse (double A[N][N],double B[N][N], int n)
{
int i, j, k;
double max, temp;
double t[N][N]; //临时矩阵
//将A矩阵存放在临时矩阵t[n][n]中
for (i = 0; i < n; i++)
{
for (j = 0; j < n; j++)
{
t[i][j] = A[i][j];
}
}
//初始化B矩阵为单位阵
for (i = 0; i < n; i++)
{
for (j = 0; j < n; j++)
{
B[i][j] = (i == j) ? (float)1 : 0;
}
}
for (i = 0; i < n; i++)
{
//寻找主元
max = t[i][i];
k = i;
for (j = i+1; j < n; j++)
{
if (fabs(t[j][i]) > fabs(max))
{
max = t[j][i];
k = j;
}
}
//如果主元所在行不是第i行,进行行交换
if (k != i)
{
for (j = 0; j < n; j++)
{
temp = t[i][j];
t[i][j] = t[k][j];
t[k][j] = temp;
//B伴随交换
temp = B[i][j];
B[i][j] = B[k][j];
B[k][j] = temp;
}
}
temp = t[i][i];
for (j = 0; j < n; j++)
{
t[i][j] = t[i][j] / temp; //主对角线上的元素变为1
B[i][j] = B[i][j] / temp; //伴随计算
}
for (j = 0; j < n; j++) //第0行->第n行
{
if (j != i) //不是第i行
{
temp = t[j][i];
for (k = 0; k < n; k++) //第j行元素 - i行元素*j列i行元素
{
t[j][k] = t[j][k] - t[i][k]*temp;
B[j][k] = B[j][k] - B[i][k]*temp;
}
}
}
}
//cout<<"***********矩阵求逆输出结果*********"<<endl;
//print(B,n,n);
}
//矩阵相乘函数
void Multiply(double a[N][N], double b[N][N], double c[N][N], int a_row, int a_column, int b_row,int b_column)
{
if(a_column==b_row)
{
for(int i=0;i<a_row;i++) //作用a矩阵的行+1
{
for(int l=0;l<b_column;l++) //作用是b矩阵的列+1
{
double k=0.0; //函数不能拿过来就用,一定要根据程序的需要改变,k的值应该是double类型不能为int类型
for(int j=0;j<a_column;j++) //作用是矩阵a的第一行元素乘以矩阵b的第一列元素
{
k=k+a[i][j]*b[j][l];
}
c[i][l]=k;
}
}
}
else cout<<"您输入的矩阵有误请重新输入……\n\n";
//cout<<"***********矩阵相乘输出结果*********"<<endl;
//print(c,a_row,b_column);
}
//矩阵相减函数
void Minus(double MOrigin1[N][N], double MOrigin2[N][N], double MResult[N][N], int m, int n)
{
int i, j;
for (i=0; i<m; i++)
for (j=0; j<n; j++)
MResult[i][j] = MOrigin1[i][j] - MOrigin2[i][j];
//cout<<"***********矩阵相减输出结果*********"<<endl;
//print(MResult,m,n);
}
//输出函数
void print(double p[N][N],int p_row,int p_column)
{
for(int i=0;i<p_row;i++)
{
cout<<setw(20);
for(int j=0;j<p_column;j++)
{
cout<<setiosflags(ios::right)
<<p[i][j]<<setw(20);
}
cout<<"\n";
}
}
//主函数
void main()
{
cout.precision(5);
const int m=50000;
const double x0=0.0, y0=0.0, f=153.24/1000.0; //内方位元素
double Zs, Xs, Ys; //外方位元素
double Sx=0.0, Sy=0.0,Sz=0.0; //要赋初值,否则系统会给定任意的一个值,计算结果会出错
int count=0,n=4;
/* cout<<"请输入控制点的个数:";
cin>>n;
double x[N],y[N],X[N],Y[N],Z[N]; //定义数组,输入像点坐标和地面控制点坐标
for(int i=0;i<n;i++)
{
cout<<"请输入第"<<(i+1)<<"个点的影像坐标 x y(mm):"<<endl;
cin>>x[i]>>y[i]; //像点坐标
x[i]=x[i]/1000.0; y[i]=y[i]/1000.0;
cout<<"请输入第"<<(i+1)<<"个点的地面坐标 X Y Z(m)"<<endl;
cin>>X[i]>>Y[i]>>Z[i]; //地面点坐标
}*/
double xy[N][N]={ {-86.15/1000, -68.99/1000,-f}, //切记:此时像点坐标z=-f,不是z=f。
{-53.40/1000, 82.21/1000,-f}, //否则,在求旋转矩阵R时,a3、b3、c3的符号会正负号相反
{-14.78/1000, -76.63/1000,-f},
{10.46/1000, 64.43/1000,-f} };
double XYZ[N][N]={ {36589.41, 25273.32, 2195.17},
{37631.08, 31324.51, 728.69},
{39100.97, 24934.98, 2386.50},
{40426.54, 30319.81, 757.31} };
double txy[N][N],xytxy[N][N], XYZ_S[N][N], TXYZ_S[N][N], XYZ_STXYZ_S[N][N];
double cos[N][N], COS[N][N], A[N][N],TA[N][N],TAA[N][N],TAA_1[N][N],L[N][N], TAL[N][N],DX[N][N];
for(int j=0; j<n; j++)
{
Sx += XYZ[j][0];
Sy += XYZ[j][1];
Sz += XYZ[j][2];
}
Xs=Sx/n; //外方位元素中线元素的近似值
Ys=Sy/n;
Zs=Sz/n+m*f;
//*******************像点余弦值**********************************
Transpose(xy,txy,n,3);
Multiply(xy,txy,xytxy,n,3,3,n);
for(int i=0;i<n-1;i++)
{
for(int j=i+1;j<n;j++)
{
cos[2*i+j-1][0]=xytxy[i][j] / (sqrt(xytxy[i][i])*sqrt(xytxy[j][j]));
}
}
//迭代开始
while((fabs(DX[0][0])>0.00001 && fabs(DX[1][0])>0.00001 && fabs(DX[2][0])>0.00001) && count<20)
{
//*******************控制点余弦值**********************************
for(int l=0;l<n;l++)
{
XYZ_S[l][0]=XYZ[l][0]-Xs; XYZ_S[l][1]=XYZ[l][1]-Ys; XYZ_S[l][2]=XYZ[l][2]-Zs;
}
Transpose(XYZ_S,TXYZ_S,n,3);
Multiply(XYZ_S,TXYZ_S,XYZ_STXYZ_S,n,3,3,n);
for(int p=0;p<n-1;p++)
{
for(int q=p+1;q<n;q++)
{
COS[2*p+q-1][0]=XYZ_STXYZ_S[p][q] / (sqrt(XYZ_STXYZ_S[p][p])*sqrt(XYZ_STXYZ_S[q][q]));
}
}
for(int g=0;g<n-1;g++)
{
for(int h=g+1;h<n;h++)
{
A[2*g+h-1][0]= -(XYZ_S[g][0]+XYZ_S[h][0])/(sqrt(XYZ_STXYZ_S[g][g])*sqrt(XYZ_STXYZ_S[h][h]))
+COS[2*g+h-1][0] * (XYZ_S[g][0]/XYZ_STXYZ_S[g][g]+XYZ_S[h][0]/XYZ_STXYZ_S[h][h]);
A[2*g+h-1][1]= -(XYZ_S[g][1]+XYZ_S[h][1])/(sqrt(XYZ_STXYZ_S[g][g])*sqrt(XYZ_STXYZ_S[h][h]))
+COS[2*g+h-1][0] * (XYZ_S[g][1]/XYZ_STXYZ_S[g][g]+XYZ_S[h][1]/XYZ_STXYZ_S[h][h]);
A[2*g+h-1][2]= -(XYZ_S[g][2]+XYZ_S[h][2])/(sqrt(XYZ_STXYZ_S[g][g])*sqrt(XYZ_STXYZ_S[h][h]))
+COS[2*g+h-1][0] * (XYZ_S[g][2]/XYZ_STXYZ_S[g][g]+XYZ_S[h][2]/XYZ_STXYZ_S[h][h]);
}
}
Transpose(A,TA,2*n-3,3);
Multiply(TA,A,TAA,3,2*n-3,2*n-3,3);
Inverse (TAA,TAA_1,3);
Minus(cos,COS,L,2*n-3,1); //因为L=-(COS-cos),所以直接L=(cos-COS)
Multiply(TA,L,TAL,3,2*n-3,2*n-3,1);
Multiply(TAA_1,TAL,DX,3,3,3,1);
Xs+=DX[0][0];
Ys+=DX[1][0];
Zs+=DX[2][0];
count++;
}//迭代结束
cout<<"**************像点坐标的余弦值cos****************"<<endl;
print(cos,2*n-3,1);
cout<<"***************像点坐标的余弦值cos***************"<<endl;
print(COS,2*n-3,1);
cout<<"******************外方位元素的改正数 DX************************"<<endl;
print(DX,3,1);
cout<<"***************外方为元素改正数的系数矩阵A*********************"<<endl;
print(A,2*n-3,3);
//****************************角元素求解*****************************************************
double lxyz[N][N],B[N][N],TB[N][N],TBB[N][N],TBlxyz[N][N],TBB_1[N][N],abc[N][N]; //abc表示旋转矩阵R
double t=0.0, w=0.0, k=0.0; //外方位元素中的角元素
for(int u=0;u<n;u++)
{
for(int v=0;v<n;v++)
{
lxyz[u][v]=xy[u][v]/sqrt(xytxy[u][u]); //lxyz表示x1/s1 ……
B[u][v]=XYZ_S[u][v]/sqrt(XYZ_STXYZ_S[u][u]); //B表示系数矩阵 (X1-XS)/S1
}
}
Transpose(B,TB,n,3);
Multiply(TB,B,TBB,3,n,n,3);
Multiply(TB,lxyz,TBlxyz,3,n,n,3); //由于坐标的输入是按照x、y、z输入。所以lxyz 第一列是x 第二列是y 第三列是z
Inverse (TBB,TBB_1,3); //所以abc=(BTB)^-1 * BTlxyz求得的第一列是 a1、b1、c1 第二列是 a2、b2、c2 第二列是 a3、b3、c3
Multiply(TBB_1,TBlxyz,abc,3,3,3,3);
//输出旋转矩阵R
评论1