houfangjiaohui.rar_源码

#include <stdio.h> #include <math.h> #define PI 3.1415926535897932 #define N 10 /************************************************************************************* * 矩阵转置函数(MResult = MOrigin(T))，参数说明: * MOrigin - 原始矩阵，以一维数组形式存储，m行n列 * MResult - 转置后矩阵，以一维数组形式存储，n行m列 *************************************************************************************/ void Transpose(double *MOrigin, double *MResult, int m, int n) { int i, j; for(i=0; i<n; i++) for(j=0; j<m; j++) MResult[i*m+j] = MOrigin[j*n+i]; } /************************************************************************************* * 矩阵求逆函数(Metrix = Metrix(-1))，参数说明: * Metrix - 原始矩阵，也是求逆之后的矩阵，以一维数组形式存储，n行n列 *************************************************************************************/ void Inverse(double *Metrix, int n) { int i, j, k; for(k=0; k<n; k++) { for(i=0; i<n; i++) { if(i != k) Metrix[i*n+k] = - Metrix[i*n+k] / Metrix[k*n+k]; } Metrix[k*n+k] = 1/Metrix[k*n+k]; for(i=0; i<n; i++) { if(i != k) { for(j=0; j<n; j++) { if(j != k) Metrix[i*n+j] += Metrix[k*n+j] * Metrix[i*n+k]; } } } for(j=0; j<n; j++) { if(j != k) Metrix[k*n+j] *= Metrix[k*n+k]; } } } /************************************************************************************* * 矩阵相乘函数(MResult = MOrigin1 * MOrigin2)，参数说明: * MOrigin1 - 原始矩阵，以一维数组形式存储，m行n列 * MOrigin2 - 原始矩阵，以一维数组形式存储，n行l列 * MResult - 相乘后矩阵，以一维数组形式存储，m行l列 *************************************************************************************/ void Multiply(double *MOrigin1, double *MOrigin2, double *MResult, int m, int n, int l) { int i, j, k; for(i=0; i<m; i++) for(j=0; j<l; j++) { MResult[i*l+j] = 0.0; for(k=0; k<n; k++) MResult[i*l+j] += MOrigin1[i*n+k] * MOrigin2[j+k*l]; } } /************************************************************************************* * 矩阵相减函数(MResult = MOrigin1 - MOrigin2)，参数说明: * MOrigin1 - 原始矩阵，以一维数组形式存储，m行n列 * MOrigin2 - 原始矩阵，以一维数组形式存储，m行n列 * MResult - 相减后矩阵，以一维数组形式存储，m行n列 *************************************************************************************/ void Minus(double *MOrigin1, double *MOrigin2, double *MResult, int m, int n) { int i, j; for (i=0; i<m; i++) for (j=0; j<n; j++) MResult[n*i+j] = MOrigin1[n*i+j] - MOrigin2[n*i+j]; } void main() { int i, m=15000, n = 6;//定义比例尺，控制点数 double phi, omega, kappa, Xs, Ys, Zs; double x0=0.0, y0=0.0, f=0.15272, Sx = 0.0, Sy = 0.0;//内方元素初值 double m0 =0.0; double x[N]={-85.938, -90.515, -92.827, 85.718,83.772,88.226}, y[N]={69.009,-6.431,-78.919, 69.711, -1.945, -73.701};//像点坐标 double X[N]={13561.393,13535.400,13515.624,16311.749,16246.429,16340.235}, Y[N]={12644.357,11444.393,10360.523,12631.929,11481.730,10314.228}, Z[N]={791.479,895.774,944.991,770.666,811.794,751.178};//地面摄影测量坐标 double H[6],a[3], b[3], c[3], X_[N], Y_[N], Z_[N], A[12*N], B[12*N], V[2*N], L[2*N], C[36], D[6], E[2*N],M[1]; phi=0; omega=0; kappa=0; Zs=m*f; for(i=0; i<n; i++) { x[i] = x[i]/1000.0; y[i] = y[i]/1000.0; Sx += x[i]; Sy += y[i]; } Xs = Sx/n; Ys = Sy/n; H[1]=Xs; H[2]=Ys; H[3]=Zs; H[4]=phi; H[5]=omega; H[6]=kappa;//确定未知数的初始值 while(fabs(H[3]) > 0.00000001 || fabs(H[4]) > 0.00000001 || fabs(H[5]) > 0.00000001) { a[0] = cos(phi)*cos(kappa) - sin(phi)*sin(omega)*sin(kappa); a[1] = -cos(phi)*sin(kappa) - sin(phi)*sin(omega)*cos(kappa); a[2] = -sin(phi)*cos(omega); b[0] = cos(omega)*sin(kappa); b[1] = cos(omega)*cos(kappa); b[2] = -sin(omega); c[0] = sin(phi)*cos(kappa) + cos(phi)*sin(omega)*sin(kappa); c[1] = -sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa); c[2] = cos(phi)*cos(omega);//旋转矩阵的值 for(i=0; i<n; i++) { X_[i] = x0 -f*(a[0]*(X[i]-Xs)+b[0]*(Y[i]-Ys)+c[0]*(Z[i]-Zs)) / (a[2]*(X[i]-Xs)+b[2]*(Y[i]-Ys)+c[2]*(Z[i]-Zs)); Y_[i] = y0 -f*(a[1]*(X[i]-Xs)+b[1]*(Y[i]-Ys)+c[1]*(Z[i]-Zs)) / (a[2]*(X[i]-Xs)+b[2]*(Y[i]-Ys)+c[2]*(Z[i]-Zs)); Z_[i] = a[2]*(X[i]-Xs) + b[2] *(Y[i]-Ys) + c[2]*(Z[i]-Zs); A[12*i+0] = (a[0]*f + a[2]*(x[i]-x0)) / Z_[i]; A[12*i+1] = (b[0]*f + b[2]*(x[i]-x0)) / Z_[i]; A[12*i+2] = (c[0]*f + c[2]*(x[i]-x0)) / Z_[i]; A[12*i+3] = (y[i]-y0)*sin(omega) - ((x[i]-x0)*((x[i]-x0)*cos(kappa) - (y[i]-y0)*sin(kappa))/f + f*cos(kappa))*cos(omega); A[12*i+4] = -f*sin(kappa) - (x[i]-x0)*((x[i]-x0)*sin(kappa) + (y[i]-y0)*cos(kappa))/f; A[12*i+5] = (y[i]-y0); A[12*i+6] = (a[1]*f + a[2]*(y[i]-y0)) / Z_[i]; A[12*i+7] = (b[1]*f + b[2]*(y[i]-y0)) / Z_[i]; A[12*i+8] = (c[1]*f + c[2]*(y[i]-y0)) / Z_[i]; A[12*i+9] = -(x[i]-x0)*sin(omega) - ((y[i]-y0)*((x[i]-x0)*cos(kappa) - (y[i]-y0)*sin(kappa))/f - f*sin(kappa))*cos(omega); A[12*i+10] = -f*cos(kappa) - (y[i]-y0)*((x[i]-x0)*sin(kappa) + (y[i]-y0)*cos(kappa))/f; A[12*i+11] = -(x[i]-x0); L[2*i] = x[i] - X_[i]; L[2*i+1] = y[i] - Y_[i];//误差方程式的系数和常数项 } Transpose(A, B, 2*n, 6); Multiply(B, A, C, 6, 2*n, 6); Multiply(B, L, D, 6, 2*n, 1); Inverse(C, 6); Multiply(C, D, H, 6, 6, 1);//求解外方位元素改正数 Xs += H[0]; Ys += H[1]; Zs += H[2]; phi += H[3]; omega += H[4]; kappa += H[5]; } Multiply(A, H, E, 2*n, 6, 1); Minus(E, L, V, 2*n, 1); Multiply(V, V, M, 2*N, 1, 1); m0 = sqrt(M[0]/(2.0*n-6.0)); printf("\n旋转矩阵R为：\n"); for (i=0; i<3; i++) printf("%.5f ", a[i]); printf("\n"); for (i=0; i<3; i++) printf("%.5f ", b[i]); printf("\n"); for (i=0; i<3; i++) printf("%.5f ", c[i]); printf("\n\n"); printf("六个外方位元素为：\n"); printf("Xs = %.4f\n", Xs); printf("Ys = %.4f\n", Ys); printf("Zs = %.4f\n", Zs); printf("phi = %.10f\n", phi); printf("omega = %.10f\n", omega); printf("kappa = %.10f\n", kappa); printf("\n单位权中误差为：m0 = %.10f\n", m0); printf("\n六个外方位元素的中误差为：\n"); for (i=0; i<6; i++) printf("m%d = %.10f\n", i+1,m0*sqrt(C[i*6+i])); }