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.

Delete this entry (admin only).