用C#编写摄影测量后方交会求外方位元素

程序源码程序源码下载地址
https://download.csdn.net/download/u011713916/11743497
实验原理


**详细题目：
**
编程思想：

具体代码
我使用了MathNet.Numerics.dll矩阵库，功能强大，还可以求解病态矩阵

MathNet.Numerics.dll

完整源码：

System;
using System.Net.Http.Headers;
using System.Xml.Linq;
using MathNet.Numerics.LinearAlgebra.Double;
using MathNet.Numerics.LinearAlgebra.Generic;//调用矩阵库



namespace The_Rescetion
{
    class Program
    {
       
        
        static void Main(string[] args)
        {
            //求单位权中误差函数
            double EeMartix  (double[,]B,double[]ll,double[]xx,int i,int j)//观测值数个数，J//求得未知数个数//B
            {
                double[] v=new double[i];//创造数组存储V；
                double q0=0;
                double Tmp = 0;//建立临时存储变量
                for (int l = 0; l < i; l++)//计算V=B*X—L
                {
                    for (int m = 0; m < j; m++)
                    {
                        Tmp = Tmp +B[l, m] * xx[m];
                    }

                    v[l] = Tmp-ll[l];
                    Tmp = 0;
                }

                for (int l = 0; l < i; l++)//计算V*VT
                {
                    Tmp += v[l] * v[l];
                }

                q0 = Math.Sqrt(Tmp / (2 * i - 6));//单位权中误差计算公式
                return q0;//返回中误差
            }
            void print( double[] a)//定义打印接口函数
            {
                for (int i = 0; i < a.Length; i++)
                {
                    Console.WriteLine(" {0}", a[i]);
                }
            }
          

           double Pinjun(double[] a)//此函数功能是取平均值
           {
               double tmp=0;
               for (int i = 1; i < a.Length; i++)
               {
                   tmp = tmp + a[i];

               }

               return (tmp / 4.0);
           }
           
            /*double[] x = new double[4];
            double[]y=new double[4];
            double[] X = new double[4];
            double[] Y = new double[4];
            double[] Z = new double[4];*/
            double[] x = new[] {-86.15, -53.40, -14.78, 10.46};
            double[] y = new []{-68.99,82.21 ,-76.63,64.43};
            double[] X = new[] { 36589.41,37631.08,39100.97,40426.54};
            double[] Y = new[] { 25273.32,31324.51,24934.98,30319.81};
            double[] Z = new[] {2195.17, 728.69, 2386.50, 757.31};
            double H = 6941.77;
           double f = 153.24;
         /*  Console.WriteLine("按列输入x");
           Fuzhi(x);
           Console.WriteLine("按列输入y");
           Fuzhi(y);
           Console.WriteLine("按列输入X");
            Fuzhi(X);
            Console.WriteLine("按列输入Y");
            Fuzhi(Y);
            Console.WriteLine("按列输入Z");
            Fuzhi(Z);
            Console.WriteLine("输入航高");
            double H = Convert.ToDouble(Console.ReadLine());
            Console.WriteLine("输入f");
            double f= Convert.ToDouble(Console.ReadLine());*/

            double Xstart=Pinjun(X) ;//赋予初值
            double Ystart =Pinjun(Y);
            double Zstart = H;//为航高
            double q=0, w=0, k = 0;//假设为垂直观测
            //double rad = Math.PI/360/60;
            double rad = 0.000000001;
            Console.WriteLine("X的初始值：{0}\nY的初始值：{1}\nZ的初始值：{2}\n",Xstart,Ystart,Zstart);
            //
            //
            double a1, a2, a3, b1, b2, b3, c1, c2, c3;//R转角矩阵
            while (true)//迭代
            {a1 = Math.Cos(q) * Math.Cos(k) - Math.Sin(q) * Math.Sin(w) * Math.Sin(k);
             a2=-Math.Cos(q)*Math.Sin(k)- Math.Sin(q) * Math.Sin(w) * Math.Sin(k);
             a3 = -Math.Sin(q) * Math.Cos(w);
             b1 = Math.Cos(w) * Math.Sin(k);
             b2 = Math.Cos(w) * Math.Cos(k);
            b3 = -Math.Sin(w);
            c1=Math.Sin(q)*Math.Cos(k)+ Math.Cos(q) * Math.Sin(w) * Math.Sin(k);
            c2 = -Math.Sin(q) * Math.Sin(k) + Math.Cos(q) * Math.Sin(w) * Math.Cos(k);
            c3 = Math.Cos(q) * Math.Cos(w);
            //
            var R=new DenseMatrix(new [,]{{a1,a2,a3},{b1,b2,b3},{c1,c2,c3}});//打印旋转矩阵
         // Console.WriteLine("旋转矩阵R为：\n{0}",R);
            //求像点近似值
            double[] l1 = new double[4];
            double[] l2 = new double[4];
            double[] l3 = new double[4];
            double[] l4= new double[4];
            double[] l5 = new double[4];
            double [,]al=new double[8,6];
            double[] ll = new double[8];
            //
            //
            //
           
                for (int i = 0; i < 4; i++)
            {
                //求像点近似值
                l1[i] = a1 * (X[i] - Xstart) + b1 * (Y[i] - Ystart) + c1 * (Z[i] - Zstart);
                l2[i] = a2 * (X[i] - Xstart) + b2 * (Y[i] - Ystart) + c2 * (Z[i] - Zstart);
                l3[i] = a3 * (X[i] - Xstart) + b3 * (Y[i] - Ystart) + c3 * (Z[i] - Zstart);
                l4[i] = -f * l1[i] / l3[i];//x点的近似坐标
                l5[i] = -f * l2[i] / l3[i];//y点的近似坐标
            }

          
           for (int i = 0; i < 4; i++)//求系数矩阵A
           {
               al[2 * i ,0] = (a1 * f + a3 * (x[i])) / l3[i];//a11
               al[2 * i ,1] = (b1 * f + b3 * (x[i] )) / l3[i];//a12
               al[2 * i ,2] = (c1 * f + c3 * (x[i] )) / l3[i];//a13
             /*a14 */  al[2 * i,3] = (y[i]) * Math.Sin(w) - Math.Cos(w) * ((x[i] ) *( (x[i]) * Math.Cos(k) - (y[i] ) * Math.Sin(k)) / f + f * Math.Cos(k));
             
             /*a15 */al[2 * i ,4] = -f * Math.Sin(k) - (x[i] ) * ((x[i]) * Math.Sin(k) + (y[i] ) * Math.Cos(k)) / f;
               al[2 * i ,5] = (y[i] );//a16
                al[2 * i+1,0] = (a2 * f + a3 * (y[i] )) / l3[i];
               al[2 * i+1,1] = (b2 * f + b3 * (y[i] )) / l3[i];
               al[2 * i+1,2] = (c2 * f + c3 * (y[i] )) / l3[i];
               al[2 * i+1,3] = -(x[i] ) * Math.Sin(w) -  Math.Cos(w) * ((y[i] ) * ((x[i] ) * Math.Cos(k) - (y[i] ) * Math.Sin(k)) / f - f * Math.Sin(k));
               al[2 * i+1,4] = -f * Math.Cos(k) - (y[i] ) * ((x[i] ) * Math.Sin(k) + (y[i] ) * Math.Cos(k)) / f;
               al[2 * i+1,5] = -(x[i] );
            }
           
           for (int i = 0; i < 4; i++)//求x-x0的矩阵L
           {
               ll[2 * i] = x[i] - l4[i];
               ll[2 * i + 1] = y[i] - l5[i];
           }
           var aMatrix=new DenseMatrix(al);//创建矩阵A
           var xmatrix =  aMatrix.Transpose()*aMatrix;//xmartix=At*A
           var Xmatrix = xmatrix.Inverse() * aMatrix.Transpose();//Xmartix=（At*A)'*At
           double[,]XXtmp=new double[6,8];
           XXtmp = Xmatrix.ToArray();//矩阵转化为二维数组
           double[]XXfinally=new double[6];//（At*A)'*At*L最终得出X的变化率
                double tmp = 0;
           for (int i = 0; i < 6; i++)
           {
               
               for (int j = 0; j < 8; j++)
               {
                   tmp = tmp + XXtmp[i, j] * ll[j];
                   
               }

               XXfinally[i] = tmp;
               tmp = 0;
           }
           //重新校正初值
           Xstart+= XXfinally[0];
           Ystart+= XXfinally[1];
           Zstart += XXfinally[2];
           q += XXfinally[3];
           w += XXfinally[4];
           k += XXfinally[5];
       
           double[] newtest = new[] { Xstart, Ystart, Zstart, q, w, k };//将新的初值存入数组
         
           if(XXfinally[3] <=rad && XXfinally[4] <= rad && XXfinally[5] <= rad)//迭代判断条件
           {
               Console.WriteLine("X,Y,Z,Q,W,k为：\n[");
                   print(newtest);//输出最后的X矩阵
                   Console.WriteLine("]");
                   Console.WriteLine("解为：\nXs={0}\nYs={1}\nZs={2}\n",newtest[0], newtest[1], newtest[2]);
               Console.WriteLine("旋转矩阵R为：\n{0}", R);
               Console.WriteLine("精度评定求得中误差为：");
             Console.WriteLine(EeMartix(al, ll, XXfinally, 8, 6));  

               break;
           }
            }

            Console.ReadKey();
        }
}
}

此代码可以修改为类直接调用，直接赋予初值数组属性即可。
支持原创，转载必须说明
——By shiZwen 2019.9.10