单片空间后方交会C++程序代码（数据来源武汉大学摄影测量学

#include <iostream>//调用头文件 double qiuni(double matrix_before[6][6], double matrix_after[6][6]);//声明定义矩阵求逆的函数 double zhuanzhi(double ai[8][6], double bi[6][8]);//声明矩阵转置的函数 double cheng(double aj[6][8], double bj[8][6], double pj[6][6]);//声明矩阵相乘1的函数 double chengq(double aj[6][6], double bj[6][8], double pj[6][8]);//声明矩阵相乘2的函数 double chengw(double an[6][8], double bn[8][1], double pn[6][1]);//声明矩阵相乘3的函数 double chengh(double an[8][6], double bn[6][1], double pn[8][1]); void main() { //已知的初始值数据 double a[4][2] = { -86.15, -68.99, -53.40, 82.21, -14.78, -76.63, 10.46, 64.43 }; double b[4][3] = { { 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 c[3][1] = { 0.0, 0.0, 153.24 }; double m = 50000.0; double f = c[2][0]/1000.0; double zo = m*f; //求初始值xo,yo,zo,q,w,k double xo = 0.0; for (int i = 0; i < 4; i++) { xo = xo + b[i][0]; } xo = xo / 4.0; double yo = 0.0; for (int i = 0; i < 4; i++) { yo = yo + b[i ][1]; } yo = yo / 4.0; double q = 0.0; double w = 0.0; double k = 0.0; double d[8][6]; double v[6][1]; double l[8][1]; double g[6][6]; //循环迭代八次求出结果 for (int iq = 0; iq < 8; iq++) { double r[3][3]; 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); double x[4]; double y[4]; double z[4]; for (int iw = 0; iw < 4; iw++) { x[iw] = -1.0 * f*(r[0][0] * (b[iw][0] - xo) + r[1][0] * (b[iw][1] - yo) + r[2][0] * (b[iw][2] - zo)) / (r[0][2] * (b[iw][0] - xo) + r[1][2] * (b[iw][1] - yo) + r[2][2] * (b[iw][2] - zo)); y[iw] = -1.0 * f*(r[0][1] * (b[iw][0] - xo) + r[1][1] * (b[iw][1] - yo) + r[2][1] * (b[iw][2] - zo)) / (r[0][2] * (b[iw][0] - xo) + r[1][2] * (b[iw][1] - yo) + r[2][2] * (b[iw][2] - zo)); z[iw]=(r[0][2] * (b[iw][0] - xo) + r[1][2] * (b[iw][1] - yo) + r[2][2] * (b[iw][2] - zo)); } //计算系数矩阵 double h; h = f*m; int iz = 0; int jz = 0; do{ d[iz][0] = (r[0][0] * f + r[0][2] * (x[jz] - c[0][0]))/ z[jz]; d[iz][1] = (r[1][0] * f + r[1][2] * (x[jz] - c[0][0])) / z[jz]; d[iz][2] = (r[2][0] * f + r[2][2] * (x[jz] - c[0][0])) / z[jz]; d[iz][3] = (y[jz] - c[1][0])*sin(w) - (((x[jz] - c[0][0]) / f)*((x[jz] - c[0][0])*cos(k) - (y[jz] - c[0][0])*sin(k)) + f*cos(k))*cos(w); d[iz][4] = -f*sin(k) - ((x[jz] - c[0][0]) / f)*((x[jz] - c[0][0])*sin(k) + (y[jz] - c[1][0])*cos(k)); d[iz][5] = y[jz] - c[1][0]; iz = iz + 1; d[iz][0] = (r[0][1] * f + r[0][2] * (y[jz] - c[0][0])) / z[jz]; d[iz][1] = (r[1][1] * f + r[1][2] * (y[jz] - c[0][0])) / z[jz]; d[iz][2] = (r[2][1] * f + r[2][2] * (y[jz] - c[0][0])) / z[jz]; d[iz][3] = -(x[jz] - c[0][0])*sin(w) - (((y[jz] - c[1][0]) / f)*((x[jz] - c[0][0])*cos(k) - (y[jz] - c[1][0])*sin(k)) - f*sin(k))*cos(w); d[iz][4] = -f*cos(k) - ((y[jz] - c[1][0]) / f)*((x[jz] - c[0][0])*sin(k) + (y[jz] - c[1][0])*cos(k)); d[iz][5] = -(x[jz] - c[0][0]); jz = jz + 1; iz = iz + 1; } while (iz < 8); double e[6][8]; zhuanzhi( d, e); //矩阵转置 double p[6][6]; cheng(e, d, p);//矩阵相乘 qiuni(p, g);//矩阵求逆 double hq[6][8]; chengq(g, e, hq);//矩阵相乘 int i2 = 0; int j2 = 0; do{ l[i2][0] = a[j2][0] / 1000.0 - x[j2]; i2 = i2 + 1; l[i2][0] = a[j2][1] / 1000.0 - y[j2]; i2 = i2 + 1; j2 = j2 + 1; } while (i2 <8); chengw(hq, l, v); //在求出改正数后加在原先的初始值上进行得到值 xo = xo + v[0][0]; yo = yo + v[1][0]; zo = zo + v[2][0]; q = q + v[3][0]; w = w + v[4][0]; k = k + v[5][0]; } printf("计算结果："); printf("\nXs=%f", xo); printf("\nYs=%f", yo); printf("\nZs=%f", zo); printf("\nq=%f", q); printf("\nw=%f", w); printf("\nk=%f", k); //精度评定 double ax[8][1]; double vl[8][1]; double mi[6][1]; chengh(d, v, ax); for (int ir = 0; ir < 8; ir++) { vl[ir][0] = ax[ir][0] - l[ir][0]; } double sume = 0.0; for (int ir3 = 0; ir3 < 8; ir3++) { sume = vl[ir3][0] * vl[ir3][0] + sume; } double mo; mo = sqrt(sume / 2); for (int ir4 = 0; ir4 < 6; ir4++) { mi[ir4][0] = (sqrt(g[ir4][ir4]))*mo; } printf("\n"); printf("\n精度评定结果："); for (int ir4 = 0; ir4 < 6; ir4++) { printf("\n%f", mi[ir4][0]); } system("pause"); }