2坐标转换 四参数/七参数/正形变换 ∈ C# 编程笔记

【日志】
2020/6/25 开了此篇博客，用于贴源码（实际上原理博客中的代码已经很全了），这篇博客只是为了方便以后复制 粘贴

一、原理

见于博客：

https://blog.csdn.net/Gou_Hailong/article/details/105362474

二、源码

源码结构图：

整个源码：

	class zbTransfer
    {
   
        /*此函数用于求d的n次方,因为用Math.Pow()太累赘
         */ 
        public static double P(double d, double n)
        {
   
            return Math.Pow(d, n);
        }
        /*此函数用于求象限角
         */ 
        public static double arctan(double dx, double dy)
        {
   
            double angle = 0;
            if(dx==0)
            {
   
                if(dy>0) angle=Math.PI*0.5;
                else     angle=Math.PI*1.5;
                return angle;
            }
            angle = Math.Atan2(Math.Abs(dy), Math.Abs(dx));
            if (dx < 0)
            {
   
                if (dy < 0) angle = Math.PI + angle;
                else        angle = Math.PI - angle;
            }
            else if (dy < 0) angle = 2 * Math.PI - angle;
            return angle;
        }
        /*此函数用于求四参数的近似值,
         * 输入：po=源坐标,pt=目标坐标
         * 输出：
         * 0 尺度因子
         * 1 旋转角度
         * 2 x平移参数
         * 3 y平移参数
         */ 
        public static double[] Get_Four_JS(Point po1, Point po2, Point pt1, Point pt2)
        {
   
            double[] JS = new double[4];
            double dx = po1.x - po2.x, dy = po1.y - po2.y;
            double dX = pt1.x - pt2.x, dY = pt1.y - pt2.y;
            double s = Math.Sqrt(dx * dx + dy * dy);
            double S = Math.Sqrt(dX * dX + dY * dY);
            double A = arctan(dX, dY);
            double a = arctan(dx, dy);
            JS[0] = s / S;
            JS[1] = A - a;
            JS[2] = pt1.x - po1.x;
            JS[3] = pt1.y - po1.y;
            return JS;
        }
        /* 此函数用于求取平面四参数 用到坐标重心化
          * 输入：po是源坐标,pt是目标坐标
          * 输出：resultMat是结果矩阵数组
          *     第一个矩阵 X      是所求的四参数
          *     第二个矩阵 V      是改正数,可以算内符合精度
          *     第三个矩阵 Sigma2 是验后单位权中误差的平方
          *     第四个矩阵 ZX     是源/目重心[po.x,po,y,pt.x,pt.y]
          */
        public static Matrix[] Get_Four(Point[] po1, Point[] pt1)
        {
   
            int n = pt1.Length, n1 = po1.Length;
            int r = 2 * n - 4;//多余观测数
            Point[] po = Point.ZXH(po1);//坐标重心化
            Point[] pt = Point.ZXH(pt1);
            double[] JS = Get_Four_JS(po[0], po[n - 1], pt[0], pt[n - 1]);//求近似
            double Dx = JS[2] ,
                Dy = JS[3],
                m = JS[0],
                Sita = JS[1];
            Matrix[] resultMat = new Matrix[4];
            Matrix B = new Matrix(2 * n, 4); ;
            Matrix P = new Matrix(2 * n); P.MakeUnitMatrix(2 * n);//默认权矩阵为单位阵
            Matrix L = new Matrix(2 * n, 1);
            Matrix X1 = new Matrix(4, 1);//参数
            while (true)
            {
   
                for (int i = 0; i < n; i++)
                {
   //根据近似参数求观测值
                    double a = Math.Cos(Sita) * po[i].x + Math.Sin(Sita) * po[i].y,
                        b = -Math.Sin(Sita) * po[i].x + Math.Cos(Sita) * po[i].y,
                        X = Dx + a * m,
                        Y = Dy - b * m;
                    B[2 * i, 0] = 1;
                    B[2 * i, 1] = 0;
                    B[2 * i, 2] = a;
                    B[2 * i, 3] = b * m;
                    B[2 * i + 1, 0] = 0;
                    B[2 * i + 1, 1] = 1;
                    B[2 * i + 1, 2] = b;
                    B[2 * i + 1, 3] = -a * m;
                    L[2 * i, 0] = pt[i].x - X;
                    L[2 * i + 1, 0] = pt[i].y - Y;
                }
                Matrix Nbb = B.Transpose() * P * B;
                Nbb.InvertGaussJordan();
                X1 = Nbb * B.Transpose() * P * L;
                X1.Eps = 1e-6;
                if (X1.Equals(0))
                    break;
                Dx += X1[0, 0];
                Dy += X1[1, 0];
                m += X1[2, 0];
                Sita += X1[3, 0];
            }           
            Matrix V = B * X1 - L;
            Matrix Sigma2 = V.Transpose() * P * V;//验后单位权中误差
            Sigma2[0, 0] /= r;

            Matrix ZX = new Matrix(4, 1);//重心矩阵
            ZX[0, 0] = po[n1].x; ZX[1, 0] = po[n1].y;
            ZX[2, 0] = pt[n].x; ZX[3, 0] = pt[n].y;

            resultMat[0] = X1;
            resultMat[1] = V;
            resultMat[2] = Sigma2;
            resultMat[3] = ZX;
            return resultMat;
        }
        /* 此函数用于求取平面四参数 未用到坐标重心化,实际上是舍去了正形变换的高阶项
          * 输入：po是源坐标,pt是目标坐标,可选参数n 取前n(>=2)点平差
          * 输出：resultMat是结果矩阵数组
          *     第一个矩阵 X      是所求的四参数
          *     第二个矩阵 V      是改正数,可以算内符合精度
          *     第三个矩阵 Sigma2 是验后单位权中误差的平方
          */
        public static Matrix[] Get_Four1(Point[] po, Point[] pt,int n=0)
        {
   
            if(n==0) n = pt.Length;
            int r = 2 * n - 4;//多余观测数
            Matrix[] resultMat = new Matrix[3];
            Matrix B = new Matrix(2 * n, 4); ;
            Matrix P = new Matrix(2 * n); P.MakeUnitMatrix(2 * n);//默认权矩阵为单位阵
            Matrix L = new Matrix(2 *