# Demo entry 6655424

py

Submitted by xxy on Oct 26, 2017 at 15:24
Language: C++. Code size: 3.9 kB.

```#include "StdAfx.h"
#include "iostream"
using namespace std;
#include<math.h>
#include "myMat.h"
#define N 4

void main()
{
double Xs,Ys,Zs,q,w,k;
double a[3],b[3],c[3];
double x0,y0,f;
double x1[N],y1[N];
double m;
int i,n=0;
double sum=0,m0;
double L[2*N];
double XX[6];
double A[2*N][6];
double X0[N],Y0[N],Z0[N],At[6][2*N],r1[6][6],r2[6][1];

//赋予题目中给的像片坐标以及地面点坐标
double x[N] = {-86.15, -53.40, -14.78, 10.46};
double y[N] = {-68.99, 82.21, -76.63, 64.43};
double X[N] = {36589.41, 37631.08, 39100.97, 40426.54};
double Y[N] = {25273.32, 31324.51, 24934.98, 30319.81};
double Z[N] = {2195.17, 728.69, 2386.50, 757.31};

//赋予外方位元素初始值

XX[3]=1;
q=0;w=0;k=0;
f=153.24;m=50000;
Xs=0;Ys=0;Zs=f*m/1000;
x0=0;y0=0;

//迭代计算
while((XX[3]>0.00001 || XX[4]>0.00001 || XX[5]>0.00001)&&n<100)
{
//旋转矩阵R
a[0]=cos(q)*cos(k)-sin(q)*sin(w)*sin(k);
a[1]=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
a[2]=-sin(q)*cos(w);
b[0]=cos(w)*sin(k);
b[1]=cos(w)*cos(k);
b[2]=-sin(w);
c[0]=sin(q)*cos(k)+cos(q)*sin(w)*sin(k);
c[1]=-sin(q)*sin(k)+cos(q)*sin(w)*cos(k);
c[2]=cos(q)*cos(w);

//像点坐标计算值
for(i=0;i<N;i++)
{
X0[i]=a[0]*(X[i]-Xs)+b[0]*(Y[i]-Ys)+c[0]*(Z[i]-Zs);
Y0[i]=a[1]*(X[i]-Xs)+b[1]*(Y[i]-Ys)+c[1]*(Z[i]-Zs);
Z0[i]=a[2]*(X[i]-Xs)+b[2]*(Y[i]-Ys)+c[2]*(Z[i]-Zs);
x1[i]=x0-f*X0[i]/Z0[i];
y1[i]=y0-f*Y0[i]/Z0[i];
}
//误差方程偏导数
for(i=0;i<N;i++)
{
A[2*i][0]=((a[0]*f+a[2]*(x[i]-x0)))/Z0[i];
A[2*i][1]=((b[0]*f+b[2]*(x[i]-x0)))/Z0[i];
A[2*i][2]=((c[0]*f+c[2]*(x[i]-x0)))/Z0[i];
A[2*i][3]=(y[i]-y0)*sin(w)-((x[i]-x0)*((x[i]-x0)*cos(k)-y[i]*sin(k))/f+f*cos(k))*cos(w);
A[2*i][4]=-f*sin(k)-(x[i]-x0)*((x[i]-x0)*sin(k)+(y[i]-y0)*cos(k))/f;
A[2*i][5]=y[i]-y0;
L[2*i]=x[i]-x1[i];

A[1+2*i][0]=((a[1]*f+a[2]*(y[i]-y0)))/Z0[i];
A[1+2*i][1]=((b[1]*f+b[2]*(y[i]-y0)))/Z0[i];
A[1+2*i][2]=((c[1]*f+c[2]*(y[i]-y0)))/Z0[i];
A[1+2*i][3]=-(x[i]-x0)*sin(w)-((y[i]-y0)*((x[i]-x0)*cos(k)-(y[i]-y0)*sin(k))/f-f*sin(k))*cos(w);
A[1+2*i][4]=-f*cos(k)-(y[i]-y0)*((x[i]-x0)*sin(k)+(y[i]-y0)*cos(k))/f;
A[1+2*i][5]=-x[i]+x0;
L[1+2*i]=y[i]-y1[i];
}

//解法方程
Matrix_Transpose(&A[0][0],&At[0][0],2*N,6);

Matrix_Multiply(&At[0][0],&A[0][0],&r1[0][0],6,2*N,6);

Matrix_Invers(&r1[0][0],6);

Matrix_Multiply(&At[0][0],L,&r2[0][0],6,2*N,1);

Matrix_Multiply(&r1[0][0],&r2[0][0],&XX[0],6,6,1);

Xs+=XX[0];
Ys+=XX[1];
Zs+=XX[2];

q+=XX[3];
w+=XX[4];
k+=XX[5];
n++;
}

//计算单位权中误差
for(i=0;i<2*N;i++)
sum+=L[i]*L[i];
m0=sqrt(sum/(2*N-6));

//计算旋转矩阵R
a[0]=cos(q)*cos(k)-sin(q)*sin(w)*sin(k);
a[1]=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
a[2]=-sin(q)*cos(w);
b[0]=cos(w)*sin(k);
b[1]=cos(w)*cos(k);
b[2]=-sin(w);
c[0]=sin(q)*cos(k)+cos(q)*sin(w)*sin(k);
c[1]=-sin(q)*sin(k)+cos(q)*sin(w)*cos(k);
c[2]=cos(q)*cos(w);

//cout<<"迭代次数为: "<<n<<endl;
printf("\n外方位元素：\n");
cout<<"航向顷角q: "<<q<<endl;
cout<<"旁向倾角w: "<<w<<endl;
cout<<"像片旋角k: "<<k<<endl;
printf("Xs=%lf\nYs=%lf\nZs=%lf\n",Xs,Ys,Zs);
cout<<"--------------------------------------------"<<endl;
cout<<endl;
printf("旋转矩阵R: \n");
for(i=0;i<3;i++)
printf("%lf\t",a[i]);
printf("\n");
for(i=0;i<3;i++)
printf("%lf\t",b[i]);
printf("\n");
for(i=0;i<3;i++)
printf("%lf\t",c[i]);
printf("\n");

cout<<"--------------------------------------------"<<endl;
cout<<"\n测量精度: "<<endl;
cout<<"Xs的精度为: "<<m0*sqrt(r1[0][0])<<endl;
cout<<"Ys的精度为: "<<m0*sqrt(r1[1][1])<<endl;
cout<<"Zs的精度为: "<<m0*sqrt(r1[2][2])<<endl;
cout<<"q的精度为: "<<m0*sqrt(r1[3][3])<<endl;
cout<<"w的精度为: "<<m0*sqrt(r1[4][4])<<endl;
cout<<"k的精度为: "<<m0*sqrt(r1[5][5])<<endl;
cout<<"--------------------------------------------"<<endl;
cout<<"\n单位权中误差为： "<<m0<<endl;
cout<<"--------------------------------------------"<<endl;
cout<<endl;
}
```

This snippet took 0.02 seconds to highlight.

Back to the Entry List or Home.