双向解析光束法

双向解析光束法光束法程序有问题，在Getelement这个函数里便出现索引超限，这个问题一直解决不了光束法的流程：1.根据同名像点对对相交理论求系数阵A，系数阵B和常数阵La11=(a1f+a3x)/Z; a12=(b1f+b3x)/Z; a13=(c1f+c3x)/Z; a14=ysin(omega)-[x/f(xcos(kappa)-ysin(kappa))+fcos(kappa)]cos(omega);a15=-fsin(kappa)-x/f(xsin(kappa)+ycos(kappa));a16=y;a21=(a2f+a3y)/Z; a22=(b2f+b3y)/Z; a23=(c2f+c3y); a24=-xsin(omega)-[x/f(xcos(kappa)-ysin(kappa))-fsin(kappa)]cos(omega)a25=-fcos(kappa)-y/f(xsin(kappa)+ycos(kappa));a26=-x;2.求方程的改化法方程求出外方位元素和物方坐标改正数3.判断改正数的值，如果小于限差则输出结果光束法是最严密的一种方法的原因：在一张相片中，待定点与控制点的像点与摄影中心及相应地面点均构成一条光束，该方法是以每张相片所组成的一束光线作为平差的基本单元，已共线条件方程作为平差的基础方程，通过各个光束在空间中的旋转和平移，使模型之间公共点的光线实现最佳交汇，并使整个区域纳入到已知的地面控制点坐标系中，所以要建立全区域统一的误差方程，整体解求全区域内每张相片的六个外方位元素及所有待定点坐标，光束法区域网平差是基于摄影时像点，物点和摄站点三点共线提出来的。

由单张相片构成区域，其平差的数学模型是共线条件方程，平差单元是单个光束，像点坐标是观测值，未知数是每张相片的外方位元素及所有待定点坐标。

误差方程直接由像点坐标的观测值列出，能对像点坐标进行系统误差改正。

光束法的程序代码为：//计算像片外方位元素，架设phi=0,omega=0,kappa=0求Xs,Ys,Zs//求左片的Xs,Ys,ZsXsl = (strX[0] + strX[2]) / 2;Ysl = (strY[0] + strY[2]) / 2;L = Math.Pow(Math.Pow(strX[0] - strX[2], 2) + Math.Pow(strY[0] - strY[2], 2) + Math.Pow(strZ[0] - strZ[2], 2), 0.5);l = Math.Pow(Math.Pow(strXl[0] - strXl[2], 2) + Math.Pow(strYl[0] - strYl[2], 2),0.5);H = f * L / l;Zsl = (strZ[0] + strZ[2]) / 2 + H;//计算片左的加密点的物方坐标Class1 xyz = new Class1(8, 3, strXY);Class1 xyzt=xyz.Transpose ();//phi,omega,kappa为零，故旋转矩阵为单位阵//像空间辅助坐标系中的坐标Class1 UVW = xyzt.Multiply(H);//求地面摄影测量坐标系中的坐标for (int i = 0; i < 8; i++){UVW.SetElement(0, i, UVW.GetElement(0, i) + Xsl);UVW.SetElement(1, i, UVW.GetElement(1, i) + Ysl);UVW.SetElement(2, i, UVW.GetElement(2, i) + Zsl);}//求右片的Xs,Ys,ZsXsr = (strX[1] + strX[3]) / 2;Ysr = (strY[1] + strY[3]) / 2;L = Math.Pow(Math.Pow(strX[1] - strX[3], 2) + Math.Pow(strY[1] - strY[3], 2) + Math.Pow(strZ[1] - strZ[3], 2), 0.5);l = Math.Pow(Math.Pow(strXr[1] - strXr[3], 2) + Math.Pow(strYr[1] - strYr[3], 2),0.5);H = f * L / l;Zsr = (strZ[1] + strZ[3]) / 2 + H;//右片加密点double[] strxy;strxy = new double[24];Console.WriteLine("请输入右片加密点的相片坐标");for (int i = 0; i < 8; i++){//strxy[0 + 3 * i] = Convert.ToDouble(Console.ReadLine());//strxy[1 + 3 * i] = Convert.ToDouble(Console.ReadLine());strxy[2 + 3 * i] = -f;}for (int i = 0; i < 24; i++){strxy[i] = strxy[i] * 0.000025;}//计算片左的加密点的物方坐标Class1 XYZ = new Class1(8, 3, strxy);Class1 XYZt=XYZ .Transpose ();//phi,omega,kappa为零，故旋转矩阵为单位阵//像空间辅助坐标系中的坐标Class1 uvw = XYZt.Multiply(H);//求地面摄影测量坐标系中的坐标for (int i = 0; i < 8; i++){uvw.SetElement(0, i, (uvw.GetElement(0, i) + Xsr+UVW.GetElement(0,i))/2);uvw.SetElement(1, i, (uvw.GetElement(1, i) + Ysr+UVW.GetElement (1,i))/2);uvw.SetElement(2, i, (uvw.GetElement(2, i) + Zsr+UVW .GetElement (2,i))/2);}//从此开始循环//光束法进行平差//循环体do{//求左片旋转矩阵Rdouble[] mtrRl;mtrRl = new double[9];mtrRl[0] = Math.Cos(phil) * Math.Cos(kappal) - Math.Sin(phil) * Math.Sin(omegal) * Math.Sin(kappal);mtrRl[1] = -Math.Cos(phil) * Math.Sin(kappal) - Math.Sin(phil) * Math.Sin(omegal) * Math.Cos(kappal);mtrRl[2] = -Math.Sin(phil) * Math.Cos(omegal);mtrRl[3] = Math.Cos(omegal) * Math.Sin(kappal);mtrRl[4] = Math.Cos(omegal) * Math.Cos(kappal);mtrRl[5] = -Math.Sin(omegal);mtrRl[6] = Math.Sin(phil) * Math.Cos(kappal) + Math.Cos(phil) * Math.Sin(omegal) * Math.Sin(kappal);mtrRl[7] = -Math.Sin(phil) * Math.Sin(kappal) + Math.Cos(phil) * Math.Sin(omegal) * Math.Cos(kappal);mtrRl[8] = Math.Cos(phil) * Math.Cos(omegal);//求右片旋转矩阵Rrdouble[] mtrRr;mtrRr = new double[9];mtrRr[0] = Math.Cos(phir) * Math.Cos(kappar) - Math.Sin(phir) * Math.Sin(omegar) * Math.Sin(kappar);mtrRr[1] = -Math.Cos(phir) * Math.Sin(kappar) - Math.Sin(phir) * Math.Sin(omegar) * Math.Cos(kappar);mtrRr[2] = -Math.Sin(phir) * Math.Cos(omegar);mtrRr[3] = Math.Cos(omegar) * Math.Sin(kappar);mtrRr[4] = Math.Cos(omegar) * Math.Cos(kappar);mtrRr[5] = -Math.Sin(omegar);mtrRr[6] = Math.Sin(phir) * Math.Cos(kappar) + Math.Cos(phir) * Math.Sin(omegar) * Math.Sin(kappar);mtrRr[7] = -Math.Sin(phir) * Math.Sin(kappar) + Math.Cos(phir) *Math.Sin(omegar) * Math.Cos(kappar);mtrRr[8] = Math.Cos(phir) * Math.Cos(omegar);//求系数阵Adouble[] mtrA;mtrA = new double[576];for (int i = 0; i < 8; i++){mtrA[0 + 48 * i] = (mtrRl[0] * f + mtrRl[2] * xyz.GetElement(i, 0)) / (mtrRl[2] * (uvw.GetElement(0, i) - Xsl) + mtrRl[5] * (uvw.GetElement(1, i) - Ysl) + mtrRl[8] * (uvw.GetElement(2, i) - Zsl));mtrA[1 + 48 * i] = (mtrRl[3] * f + mtrRl[5] * xyz.GetElement(i, 0)) / (mtrRl[2] * (uvw.GetElement(0, i) - Xsl) + mtrRl[5] * (uvw.GetElement(1, i) - Ysl) + mtrRl[8] * (uvw.GetElement(2, i) - Zsl));mtrA[2 + 48 * i] = (mtrRl[6] * f + mtrRl[8] * xyz.GetElement(i, 0)) / (mtrRl[2] * (uvw.GetElement(0, i) - Xsl) + mtrRl[5] * (uvw.GetElement(1, i) - Ysl) + mtrRl[8] * (uvw.GetElement(2, i) - Zsl));mtrA[3 + 48 * i] = xyz.GetElement(i, 1) * Math.Sin(omegal) - (xyz.GetElement(i, 0) * (xyz.GetElement(i, 0) * Math.Cos(kappal) - xyz.GetElement(i, 1) * Math.Sin(kappal) / f) + f * Math.Cos(kappal)) * Math.Cos(omegal);mtrA[4 + 48 * i] = -f * Math.Sin(kappal) - xyz.GetElement(i, 0) * (xyz.GetElement(i, 0) * Math.Sin(kappal) + xyz.GetElement(i, 1) * Math.Cos(kappal)) / f;mtrA[5 + 48 * i] = xyz.GetElement(i, 1);for (int j = 0; j < 6; j++){mtrA[j + 6 + 48 * i] = 0;}mtrA[12 + 48 * i] = (mtrRl[1] * f + mtrRl[2] * xyz.GetElement(i, 1)) / (mtrRl[2] * (uvw.GetElement(0, i) - Xsl) + mtrRl[5] * (uvw.GetElement(1, i) - Ysl) + mtrRl[8] * (uvw.GetElement(2, i) - Zsl));mtrA[13 + 48 * i] = (mtrRl[4] * f + mtrRl[5] * xyz.GetElement(i, 1)) / (mtrRl[2] * (uvw.GetElement(0, i) - Xsl) + mtrRl[5] * (uvw.GetElement(1, i) - Ysl) + mtrRl[8] * (uvw.GetElement(2, i) - Zsl));mtrA[14 + 48 * i] = (mtrRl[7] * f + mtrRl[8] * xyz.GetElement(i, 1)) / (mtrRl[2] * (uvw.GetElement(0, i) - Xsl) + mtrRl[5] * (uvw.GetElement(1, i) - Ysl) + mtrRl[8] * (uvw.GetElement(2, i) - Zsl));mtrA[15 + 48 * i] = -xyz.GetElement(i, 0) * Math.Sin(omegal) - (xyz.GetElement(i, 0) * (xyz.GetElement(i, 0) * Math.Cos(kappal) - xyz.GetElement(i, 1) * Math.Sin(kappal)) / f - f * Math.Sin(kappal)) * Math.Cos(omegal);mtrA[16 + 48 * i] = -f * Math.Cos(kappal) - xyz.GetElement(i, 1) * (xyz.GetElement(i, 0) * Math.Sin(kappal) + xyz.GetElement(i, 1) * Math.Cos(kappal)) / f;mtrA[17 + 48 * i] = -xyz.GetElement(i, 0);for (int j = 0; j < 12; j++){mtrA[j + 18 + 48 * i] = 0;}mtrA[30 + 48 * i] = (mtrRr[0] * f + mtrRr[2] * XYZ.GetElement(i, 0)) / (mtrRr[2] * (uvw.GetElement(0, i) - Xsr) + mtrRr[5] * (uvw.GetElement(1, i) - Ysr) + mtrRr[8] * (uvw.GetElement(2, i) - Zsr));mtrA[31 + 48 * i] = (mtrRr[3] * f + mtrRr[5] * XYZ.GetElement(i, 0)) / (mtrRr[2] * (uvw.GetElement(0, i) - Xsr) + mtrRr[5] * (uvw.GetElement(1, i) - Ysr) + mtrRr[8] * (uvw.GetElement(2, i) - Zsr));mtrA[32 + 48 * i] = (mtrRr[6] * f + mtrRr[8] * XYZ.GetElement(i, 0)) / (mtrRr[2] * (uvw.GetElement(0, i) - Xsr) + mtrRr[5] * (uvw.GetElement(1, i) - Ysr) + mtrRr[8] * (uvw.GetElement(2, i) - Zsr));mtrA[33 + 48 * i] = XYZ.GetElement(i, 1) * Math.Sin(omegar) - (XYZ.GetElement(i, 0) * (XYZ.GetElement(i, 0) * Math.Cos(kappar) - XYZ.GetElement(i, 1) * Math.Sin(kappar) / f) + f * Math.Cos(kappar)) * Math.Cos(omegar);mtrA[34 + 48 * i] = -f * Math.Sin(kappar) - XYZ.GetElement(i, 0) * (XYZ.GetElement(i, 0) * Math.Sin(kappar) + XYZ.GetElement(i, 1) * Math.Cos(kappar)) / f;mtrA[35 + 48 * i] = XYZ.GetElement(i, 1);for (int j = 0; j < 6; j++){mtrA[j + 36 + 48 * i] = 0;}mtrA[42 + 48 * i] = (mtrRr[1] * f + mtrRr[2] * XYZ.GetElement(i, 1)) / (mtrRr[2] * (uvw.GetElement(0, i) - Xsr) + mtrRr[5] * (uvw.GetElement(1, i) - Ysr) + mtrRr[8] * (uvw.GetElement(2, i) - Zsr));mtrA[43 + 48 * i] = (mtrRr[4] * f + mtrRr[5] * XYZ.GetElement(i, 1)) / (mtrRr[2] * (uvw.GetElement(0, i) - Xsr) + mtrRr[5] * (uvw.GetElement(1, i) - Ysr) + mtrRr[8] * (uvw.GetElement(2, i) - Zsr));mtrA[44 + 48 * i] = (mtrRr[7] * f + mtrRr[8] * XYZ.GetElement(i, 1)) / (mtrRr[2] * (uvw.GetElement(0, i) - Xsr) + mtrRr[5] * (uvw.GetElement(1, i) - Ysr) + mtrRr[8] * (uvw.GetElement(2, i) - Zsr));mtrA[45 + 48 * i] = -XYZ.GetElement(i, 0) * Math.Sin(omegar) - (XYZ.GetElement(i, 0) * (XYZ.GetElement(i, 0) * Math.Cos(kappar) - XYZ.GetElement(i, 1) * Math.Sin(kappar)) / f - f * Math.Sin(kappar)) * Math.Cos(omegar);mtrA[46 + 48 * i] = -f * Math.Cos(kappar) - XYZ.GetElement(i, 1) * (XYZ.GetElement(i, 0) * Math.Sin(kappar) + XYZ.GetElement(i, 1) * Math.Cos(kappar)) / f;mtrA[47 + 48 * i] = -XYZ.GetElement(i, 0);}for (int i = 8; i < 12; i++){mtrA[0 + 48 * i] = (mtrRl[0] * f + mtrRl[2] * strXl[i - 8]) / (mtrRl[2] * (strX[i - 8] - Xsl) + mtrRl[5] * (strY[i - 8] - Ysl) + mtrRl[8] * (strZ[i - 8] - Zsl));mtrA[1 + 48 * i] = (mtrRl[3] * f + mtrRl[5] * strXl[i - 8]) / (mtrRl[2] * (strX[i - 8] - Xsl) + mtrRl[5] * (strY[i - 8] - Ysl) + mtrRl[8] * (strZ[i - 8] - Zsl));mtrA[2 + 48 * i] = (mtrRl[6] * f + mtrRl[8] * strXl[i - 8]) / (mtrRl[2] * (strX[i - 8] - Xsl) + mtrRl[5] * (strY[i - 8] - Ysl) + mtrRl[8] * (strZ[i - 8] - Zsl));mtrA[3 + 48 * i] = strYl[i - 8] * Math.Sin(omegal) - (strXl[i - 8] * (strXl[i - 8] * Math.Cos(kappal) - strYl[i - 8] * Math.Sin(kappal) / f) + f * Math.Cos(kappal)) * Math.Cos(omegal);mtrA[4 + 48 * i] = -f * Math.Sin(kappal) - strXl[i - 8] * (strXl[i - 8] * Math.Sin(kappal) + strYl[i - 8] * Math.Cos(kappal)) / f;mtrA[5 + 48 * i] = strYl[i - 8];for (int j = 0; j < 6; j++){mtrA[j + 6 + 48 * i] = 0;}mtrA[12 + 48 * i] = (mtrRl[1] * f + mtrRl[2] * strYl[i - 8]) / (mtrRl[2] * (strX[i - 8] - Xsl) + mtrRl[5] * (strY[i - 8] - Ysl) + mtrRl[8] * (strZ[i - 8] - Zsl));mtrA[13 + 48 * i] = (mtrRl[4] * f + mtrRl[5] * strYl[i - 8]) / (mtrRl[2] * (strX[i - 8] - Xsl) + mtrRl[5] * (strY[i - 8] - Ysl) + mtrRl[8] * (strZ[i - 8] - Zsl));mtrA[14 + 48 * i] = (mtrRl[7] * f + mtrRl[8] * strYl[i - 8]) / (mtrRl[2] * (strX[i - 8] - Xsl) + mtrRl[5] * (strY[i - 8] - Ysl) + mtrRl[8] * (strZ[i - 8] - Zsl));mtrA[15 + 48 * i] = -strXl[i - 8] * Math.Sin(omegal) - (strXl[i - 8] * (strXl[i - 8] * Math.Cos(kappal) - strYl[i - 8] * Math.Sin(kappal)) / f - f * Math.Sin(kappal)) * Math.Cos(omegal);mtrA[16 + 48 * i] = -f * Math.Cos(kappal) - strYl[i - 8] * (strXl[i - 8] * Math.Sin(kappal) + strYl[i - 8] * Math.Cos(kappal)) / f;mtrA[17 + 48 * i] = -strXl[i - 8];for (int j = 0; j < 12; j++){mtrA[j + 18 + 48 * i] = 0;}mtrA[30 + 48 * i] = (mtrRr[0] * f + mtrRr[2] * strXr[i - 8]) / (mtrRr[2] * (strX[i - 8] - Xsr) + mtrRr[5] * (strY[i - 8] - Ysr) + mtrRr[8] * (strZ[i - 8] - Zsr));mtrA[31 + 48 * i] = (mtrRr[3] * f + mtrRr[5] * strXr[i - 8]) / (mtrRr[2] * (strX[i - 8] - Xsr) + mtrRr[5] * (strY[i - 8] - Ysr) + mtrRr[8] * (strZ[i - 8] - Zsr));mtrA[32 + 48 * i] = (mtrRr[6] * f + mtrRr[8] * strXr[i - 8]) / (mtrRr[2] * (strX[i - 8] - Xsr) + mtrRr[5] * (strY[i - 8] - Ysr) + mtrRr[8] * (strZ[i - 8] - Zsr));mtrA[33 + 48 * i] = strYr[i - 8] * Math.Sin(omegar) - (strXr[i - 8] * (strXr[i - 8] * Math.Cos(kappar) - strYr[i - 8] * Math.Sin(kappar) / f) + f * Math.Cos(kappar)) * Math.Cos(omegar);mtrA[34 + 48 * i] = -f * Math.Sin(kappar) - strXr[i - 8] * (strXr[i - 8] * Math.Sin(kappar) + strYr[i - 8] * Math.Cos(kappar)) / f;mtrA[35 + 48 * i] = strYr[i - 8];for (int j = 0; j < 6; j++){mtrA[j + 36 + 48 * i] = 0;}mtrA[42 + 48 * i] = (mtrRr[1] * f + mtrRr[2] * strYr[i - 8]) / (mtrRr[2] * (strX[i - 8] - Xsr) + mtrRr[5] * (strY[i - 8] - Ysr) + mtrRr[8] * (strZ[i - 8] - Zsr));mtrA[43 + 48 * i] = (mtrRr[4] * f + mtrRr[5] * strYr[i - 8]) / (mtrRr[2] * (strX[i - 8] - Xsr) + mtrRr[5] * (strY[i - 8] - Ysr) + mtrRr[8] * (strZ[i - 8] - Zsr));mtrA[44 + 48 * i] = (mtrRr[7] * f + mtrRr[8] * strYr[i - 8]) / (mtrRr[2] * (strX[i - 8] - Xsr) + mtrRr[5] * (strY[i - 8] - Ysr) + mtrRr[8] * (strZ[i - 8] - Zsr));mtrA[45 + 48 * i] = -strXr[i - 8] * Math.Sin(omegar) - (strXr[i - 8] * (strXr[i - 8] * Math.Cos(kappar) - strYr[i - 8] * Math.Sin(kappar)) / f - f * Math.Sin(kappar)) * Math.Cos(omegar);mtrA[46 + 48 * i] = -f * Math.Cos(kappar) - strYr[i - 8] * (strXr[i - 8] * Math.Sin(kappar) + strYr[i - 8] * Math.Cos(kappar)) / f;mtrA[47 + 48 * i] = -strXr[i - 8];}//将数组A转化为矩阵Class1 A = new Class1(48, 6, mtrA);//求系数阵Bdouble[] mtrB;mtrB = new double[1728];for (int i = 0; i < 1728; i++){mtrB[i] = 0;//给B矩阵赋值}for (int i = 0; i < 8; i++){mtrB[0 + 147 * i] = -mtrA[0 + 48 * i];mtrB[1 + 147 * i] = -mtrA[1 + 48 * i];mtrB[2 + 147 * i] = -mtrA[2 + 48 * i];mtrB[36 + 147 * i] = -mtrA[12 + 48 * i];mtrB[37 + 147 * i] = -mtrA[13 + 48 * i];mtrB[38 + 147 * i] = -mtrA[14 + 48 * i];mtrB[72 + 147 * i] = -mtrA[30 + 48 * i];mtrB[73 + 147 * i] = -mtrA[31 + 48 * i];mtrB[74 + 147 * i] = -mtrA[32 + 48 * i];mtrB[108 + 147 * i] = -mtrA[42 + 48 * i];mtrB[109 + 147 * i] = -mtrA[43 + 48 * i];mtrB[110 + 147 * i] = -mtrA[44 + 48 * i];}//转化矩阵BClass1 B = new Class1(48, 36, mtrB);//求常数项double[] mtrL;mtrL = new double[48];for (int i = 0; i < 8; i++){mtrL[0 + 4 * i] = xyz.GetElement(i, 0) + f * (mtrRl[0] * (uvw.GetElement(0, i) - Xsl) + mtrRl[3] * (uvw.GetElement(1, i) - Ysl) + mtrRl[6] * (uvw.GetElement(2, i) - Zsl)) / (mtrRl[2] * (uvw.GetElement(0, i) - Xsl) + mtrRl[5] * (uvw.GetElement(1, i) - Ysl) + mtrRl[8] * (uvw.GetElement(2, i) - Zsl));mtrL[1 + 4 * i] = xyz.GetElement(i, 1) + f * (mtrRl[1] * (uvw.GetElement(0, i) - Xsl) + mtrRl[4] * (uvw.GetElement(1, i) - Ysl) + mtrRl[7] * (uvw.GetElement(2, i) - Zsl)) / (mtrRl[2] * (uvw.GetElement(0, i) - Xsl) + mtrRl[5] * (uvw.GetElement(1, i) - Ysl) + mtrRl[8] * (uvw.GetElement(2, i) - Zsl));mtrL[2 + 4 * i] = XYZ.GetElement(i, 0) + f * (mtrRr[0] * (uvw.GetElement(0, i) - Xsr) + mtrRr[3] * (uvw.GetElement(1, i) - Ysr) + mtrRr[6] * (uvw.GetElement(2, i) - Zsr)) / (mtrRr[2] * (uvw.GetElement(0, i) - Xsr) + mtrRr[5] * (uvw.GetElement(1, i) - Ysr) + mtrRr[8] * (uvw.GetElement(2, i) - Zsr));mtrL[3 + 4 * i] = XYZ.GetElement(i, 1) + f * (mtrRr[1] * (uvw.GetElement(0, i) - Xsr) + mtrRr[4] * (uvw.GetElement(1, i) - Ysr) + mtrRr[7] * (uvw.GetElement(2, i) - Zsr)) / (mtrRr[2] * (uvw.GetElement(0, i) - Xsr) + mtrRr[5] * (uvw.GetElement(1, i) - Ysr) + mtrRr[8] * (uvw.GetElement(2, i) - Zsr));}for (int i = 8; i < 12; i++){mtrL[0 + 4 * i] = strXl[i - 8] + f * (mtrRl[0] * (strX[i - 8] - Xsl) + mtrRl[3] * (strY[i - 8] - Ysl) + mtrRl[6] * (strZ[i - 8] - Zsl)) / (mtrRl[2] * (strX[i - 8] - Xsl) + mtrRl[5] * (strY[i - 8] - Ysl) + mtrRl[8] * (strZ[i - 8] - Zsl));mtrL[1 + 4 * i] = strYl[i - 8] + f * (mtrRl[1] * (strX[i - 8] - Xsl) + mtrRl[4] * (strY[i - 8] - Ysl) + mtrRl[7] * (strZ[i - 8] - Zsl)) / (mtrRl[2] * (strX[i - 8] - Xsl) + mtrRl[5] * (strY[i - 8] - Ysl) + mtrRl[8] * (strZ[i - 8] - Zsl));mtrL[2 + 4 * i] = strXr[i - 8] + f * (mtrRr[0] * (strX[i - 8] - Xsr) + mtrRr[3] * (strY[i - 8] - Ysr) + mtrRr[6] * (strZ[i - 8] - Zsr)) / (mtrRr[2] * (strX[i - 8] - Xsr) + mtrRr[5] * (strY[i - 8] - Ysr) + mtrRr[8] * (strZ[i - 8] - Zsr));mtrL[3 + 4 * i] = strYr[i - 8] + f * (mtrRr[1] * (strX[i - 8] - Xsr) + mtrRr[4] * (strY[i - 8] - Ysr) + mtrRr[7] * (strZ[i - 8] - Zsr)) / (mtrRr[2] * (strX[i - 8] - Xsr) + mtrRr[5] * (strY[i - 8] - Ysr) + mtrRr[8] * (strZ[i - 8] - Zsr));}//转化矩阵LClass1 LL = new Class1(48, 1, mtrL);//求改化法方程的系数Class1 N11 = A.Transpose().Multiply(A);Class1 N12 = A.Transpose().Multiply(B);Class1 N21 = B.Transpose().Multiply(A);Class1 N22 = B.Transpose().Multiply(B);Class1 u1 = A.Transpose().Multiply(LL);Class1 u2 = B.Transpose().Multiply(LL);//求外方位元素//改化法方程系数N22.InvertGaussJordan();Class1 n1 = N11.Subtract(N12.Multiply(N22).Multiply(N12.Transpose()));//常数项Class1 lt = u1.Subtract(N12.Multiply(N22).Multiply(u2));//求外方位元素改正数n1.InvertGaussJordan();Class1 t = n1.Multiply(lt);//求物方坐标改正数//改化法方程系数N22.InvertGaussJordan();N11.InvertGaussJordan();Class1 n2 = N22.Subtract(N12.Transpose().Multiply(N11).Multiply(N12));Class1 lX = u2.Subtract(N12.Transpose().Multiply(N11).Multiply(u1));//求物方坐标改正数n2.InvertGaussJordan();Class1 X = n2.Multiply(lX);count++;//改正外方位元素和物方坐标phil += t.GetElement(0, 0);omegal += t.GetElement(1, 0);kappal += t.GetElement(2, 0);Xsl += t.GetElement(3, 0);Ysl += t.GetElement(4, 0);Zsl += t.GetElement(5, 0);phir += t.GetElement(6, 0);omegar += t.GetElement(7, 0);kappar += t.GetElement(8, 0);Xsr += t.GetElement(9, 0);Ysr += t.GetElement(10, 0);Zsr += t.GetElement(11, 0);//物方坐标改正数for (int j = 0; j < 8; j++){uvw.SetElement(0, j, uvw.GetElement(0, j) + X.GetElement(0 + 3 * j, 0));uvw.SetElement(1, j, uvw.GetElement(1, j) + X.GetElement(1 + 3 * j, 0));uvw.SetElement(2, j, uvw.GetElement(2, j) + X.GetElement(2 + 3 * j, 0));}//将改正数带出作结束判断for (int j = 0; j < 12; j++){dett[j] = t.GetElement(j, 0);}for (int j = 0; j < 24; j++){detxyz[j] = X.GetElement(j, 0);}for (int i = 0; i < 12; i++){if (det1 < dett[i])det1 = dett[i];}for (int i = 0; i < 24; i++){if (det2 < detxyz[i])det2 = detxyz[i];}}while (det1 > 0.000001 && det2 > 0.000001); Console.WriteLine("count={0}", count); Console.ReadLine();。