地理空間座標系統-不同橢球基準間的座標轉換-相關算法代碼實現C#

在上一篇的論述中，不同橢球基準間的座標轉換除了同一個橢球基準內座標轉換算法外，我們還提到了兩個算法：

1.已知兩個空間直角座標間轉換的七參數△X，△Y，△Z，ωx，ωy，ωz，m的情況情況下，如何進行橢球轉換。

2.在無法獲取兩個空間直角座標轉換所需的七參數的情況下，如何通過三個以上的座標點對，反算七參數。

已七參數，進行橢球轉換

根據上一篇中的公式，具體代碼如下：

       /// <summary>
        /// 空間直角座標之間的轉換，使用布爾莎七參轉換
        /// </summary>
        /// <param name="originalPoint"></param>
        /// <param name="dx">X軸平移參數，單位"米"</param>
        /// <param name="dy">Y軸平移參數，單位"米"</param>
        /// <param name="dz">Z軸平移參數，單位"米"</param>
        /// <param name="rotax">旋轉參數x,單位"弧度"</param>
        /// <param name="rotay">旋轉參數y,單位"弧度"</param>
        /// <param name="rotaz">旋轉參數z,單位"弧度"</param>
        /// <param name="scale">尺度參數k</param>
        /// <param name="reslutPoint">返回的座標</param>
        public static void SpaceRectangularToSpaceRectangularByBursa(
           Point3d originalPoint, 
           double dx,double dy,double dz,double rotax,double rotay,double rotaz,
           double scale, out Point3d reslutPoint)
        {
            double a1 = scale + 1;
            double a2 = a1 * rotax;
            double a3 = a1 * rotay;
            double a4 = a1 * rotaz;

            reslutPoint.X = dx + a1 * originalPoint.X 
                  - a3 * originalPoint.Z + a4 * originalPoint.Y;
            reslutPoint.Y = dy + a1 * originalPoint.Y 
                  + a2 * originalPoint.Z - a4 * originalPoint.X;  
            reslutPoint.Z = dz + a1 * originalPoint.Z 
                  - a2 * originalPoint.Y + a3 * originalPoint.X; 


            //B=AX，下面是想用矩陣計算的方式，來對結果進行驗證。可以忽略

            double[,] pX = new double[7, 1];
            double[,] pA = new double[3, 7];

            pX[0, 0] = dx;
            pX[1, 0] = dy;
            pX[2, 0] = dz;
            pX[3, 0] = a1;
            pX[4, 0] = a2;
            pX[5, 0] = a3;
            pX[6, 0] = a4;

            ///初始化A矩陣第一步
            for (int i = 0; i <  3; i++)
            {
                if (i % 3 == 0)
                {
                    pA[i, 0] = 1;
                    pA[i, 1] = 0;
                    pA[i, 2] = 0;
                    pA[i, 3] = originalPoint.X;
                    pA[i, 4] = 0;
                    pA[i, 5] = -originalPoint.Z;
                    pA[i, 6] = originalPoint.Y;
                }
                else if (i % 3 == 1)
                {
                    pA[i, 0] = 0;
                    pA[i, 1] = 1;
                    pA[i, 2] = 0;
                    pA[i, 3] = originalPoint.Y;
                    pA[i, 4] = originalPoint.Z;
                    pA[i, 5] = 0;
                    pA[i, 6] = -originalPoint.X;
                }
                else if (i % 3 == 2)
                {
                    pA[i, 0] = 0;
                    pA[i, 1] = 0;
                    pA[i, 2] = 1;
                    pA[i, 3] = originalPoint.Z;
                    pA[i, 4] = -originalPoint.Y;
                    pA[i, 5] = originalPoint.X;
                    pA[i, 6] = 0;
                }
            }

            GMatrix X = new GMatrix(pX);
            GMatrix A = new GMatrix(pA);
            GMatrix B = A*X;
}

未知七參數，通過三個以上的座標點對，反算七參數

根據上一篇的中的公式，具體代碼實現如下：

 /// <summary>
        /// 七參數反算
        /// </summary>
        /// <param name="originalCooords 舊座標"></param>
        /// <param name="targetCooords 新座標"></param>
        /// <param name="dx">X軸平移參數，單位"米"</param>
        /// <param name="dy">Y軸平移參數，單位"米"</param>
        /// <param name="dz">Z軸平移參數，單位"米"</param>
        /// <param name="rotax">旋轉參數x,單位"弧度"</param>
        /// <param name="rotay">旋轉參數y,單位"弧度"</param>
        /// <param name="rotaz">旋轉參數z,單位"弧度"</param>
        /// <param name="scale">尺度參數k,</param>
        public static void CalSevenParaByTwoSpaceRectangularCoords(
            Point3d[] originalCooords,Point3d[] targetCooords, 
            ref double dx, ref double dy, ref double dz, 
            ref double rotax, ref double rotay, ref double rotaz, ref double scale)
        {
         
            int pointCount = originalCooords.Length;
            if (pointCount < 3)
            {
                //座標點數小於三。
                return;
            }
            if (targetCooords.Length < pointCount)
            {
                //新舊座標個數不匹配
                return;
            }
            double[,] pA = new double[pointCount * 3, 7];
            double[,] pB = new double[pointCount * 3, 1];

            ///初始化A矩陣第一步
            for (int i=0;i< pointCount*3;i++) {
                if (i % 3 == 0)
                {
                    pA[i, 0] = 1;
                    pA[i, 1] = 0;
                    pA[i, 2] = 0;
                    pA[i, 3] = originalCooords[i / 3].X;
                    pA[i, 4] = 0;
                    pA[i, 5] = -originalCooords[i / 3].Z;
                    pA[i, 6] = originalCooords[i / 3].Y;
                }
                else if (i % 3 == 1) {
                    pA[i, 0] = 0;
                    pA[i, 1] = 1;
                    pA[i, 2] = 0;
                    pA[i, 3] = originalCooords[i / 3].Y;
                    pA[i, 4] = originalCooords[i / 3].Z;
                    pA[i, 5] = 0;
                    pA[i, 6] = -originalCooords[i / 3].X;
                }
                else if (i % 3 == 2)
                {
                    pA[i, 0] = 0;
                    pA[i, 1] = 0;
                    pA[i, 2] = 1;
                    pA[i, 3] = originalCooords[i / 3].Z;
                    pA[i, 4] = -originalCooords[i / 3].Y;
                    pA[i, 5] = originalCooords[i / 3].X;
                    pA[i, 6] = 0;
                }
            }

            ///初始化B矩陣第一步
            for (int i = 0; i < pointCount * 3; i++)
            {
                if (i % 3 == 0)
                {
                    pB[i, 0] = targetCooords[i / 3].X;
                }
                else if (i % 3 == 1)
                {
                    pB[i, 0] = targetCooords[i / 3].Y;
                }
                else if (i % 3 == 2)
                {
                    pB[i, 0] = targetCooords[i / 3].Z;
                }
            }

            GMatrix A = new GMatrix(pA);
            GMatrix AT = A.Transpose();
            GMatrix B = new GMatrix(pB);
            GMatrix W = AT * A;
            GMatrix W1 = W.Inverse();
            GMatrix reslut = W1 * AT * B;

            GMatrix m = ((A.Transpose() * A).Inverse())* A.Transpose() * B;

            //規範化
            double Xdelta, Ydelta, Zdelta,Scale, Ex, Ey, Ez;
            Xdelta = reslut[0, 0];
            Ydelta = reslut[1, 0];
            Zdelta = reslut[2, 0];
            Scale = reslut[3, 0];
            Ex = reslut[4, 0] / Scale;
            Ey= reslut[5, 0] / Scale;
            Ez = reslut[6, 0] / Scale;

            //亂的
            double a1, a2, a3, a4;
            dx = reslut[0, 0];
            dy = reslut[1, 0];
            dz = reslut[2, 0];

            a1= reslut[3, 0];
            a2 = reslut[4, 0];
            a3= reslut[5, 0];
            a4= reslut[6, 0]; 

            scale = a1-1;  //m
            rotax = a2 / a1;  //wx
            rotay = a3 / a1;  //wy
            rotaz = a4 / a1;  //wz

            //需要考慮cosA=0 不能作爲分母的情況。
        }

矩陣運算類

上面代碼中用到了矩陣，爲不去引用第三方數據庫，這裏同時分享一個矩陣運算類，網上找的，親測挺好用，謝謝庫的作者

using System;
using System.Collections.Generic;


namespace GB.GMath
{

    [Serializable]
    public class GMatrix
    {
        public double[] element;
        private int rows = 0;
        private int cols = 0;
        /// <summary>
        /// 獲取矩陣行數
        /// </summary>
        public int Rows
        {
            get
            {
                return rows;
            }
        }
        /// <summary>
        /// 獲取矩陣列數
        /// </summary>
        public int Cols
        {
            get
            {
                return cols;
            }
        }
        /// <summary>
        /// 獲取或設置第i行第j列的元素值
        /// </summary>
        /// <param name="i">第i行</param>
        /// <param name="j">第j列</param>
        /// <returns>返回第i行第j列的元素值</returns>
        public double this[int i, int j]
        {
            get
            {
                if (i < Rows && j < Cols)
                {
                    return element[i * cols + j];
                }
                else
                {
                    throw new Exception("索引越界");
                }
            }
            set
            {
                element[i * cols + j] = value;
            }
        }
        /// <summary>
        /// 用二維數組初始化GMatrix
        /// </summary>
        /// <param name="m">二維數組</param>
        public GMatrix(double[][] m)
        {
            this.rows = m.GetLength(0);
            this.cols = m.GetLength(1);
            int count = 0;
            this.element = new double[Rows * Cols];
            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    element[count++] = m[i][j];
                }
            }
        }
        public GMatrix(double[,] m)
        {
            this.rows = m.GetLength(0);
            this.cols = m.GetLength(1);
            this.element = new double[this.rows * this.cols];
            int count = 0;
            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    element[count++] = m[i, j];
                }
            }
        }
        public GMatrix(List<List<double>> m)
        {
            this.rows = m.Count;
            this.cols = m[0].Count;
            this.element = new double[Rows * Cols];
            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    this[i, j] = m[i][j];
                }
            }
        }
        #region 矩陣數學運算
        public static GMatrix MAbs(GMatrix a)
        {
            GMatrix _thisCopy = a.DeepCopy();
            for (int i = 0; i < a.Rows; i++)
            {
                for (int j = 0; j < a.Cols; j++)
                {
                    _thisCopy[i, j] = System.Math.Abs(a[i, j]);
                  
                }
            }
            return _thisCopy;
        }
        /// <summary>
        /// 矩陣相加
        /// </summary>
        /// <param name="a">第一個矩陣,和b矩陣必須同等大小</param>
        /// <param name="b">第二個矩陣</param>
        /// <returns>返回矩陣相加後的結果</returns>
        public static GMatrix operator +(GMatrix a, GMatrix b)
        {
            if (a.cols == b.cols && a.rows == b.rows)
            {
                double[,] res = new double[a.rows, a.cols];
                for (int i = 0; i < a.Rows; i++)
                {
                    for (int j = 0; j < a.Cols; j++)
                    {
                        res[i, j] = a[i, j] + b[i, j];
                    }
                }
                return new GMatrix(res);
            }
            else
            {
                throw new Exception("兩個矩陣行列不相等");
            }
        }
        /// <summary>
        /// 矩陣相減
        /// </summary>
        /// <param name="a">第一個矩陣,和b矩陣必須同等大小</param>
        /// <param name="b">第二個矩陣</param>
        /// <returns>返回矩陣相減後的結果</returns>
        public static GMatrix operator -(GMatrix a, GMatrix b)
        {
            if (a.cols == b.cols && a.rows == b.rows)
            {
                double[,] res = new double[a.rows, a.cols];
                for (int i = 0; i < a.Rows; i++)
                {
                    for (int j = 0; j < a.Cols; j++)
                    {
                        res[i, j] = a[i, j] - b[i, j];
                    }
                }
                return new GMatrix(res);
            }
            else
            {
                throw new Exception("兩個矩陣行列不相等");
            }
        }
        /// <summary>
        /// 對矩陣每個元素取相反數
        /// </summary>
        /// <param name="a">二維矩陣</param>
        /// <returns>得到矩陣的相反數</returns>
        public static GMatrix operator -(GMatrix a)
        {
            GMatrix res = a;
            for (int i = 0; i < a.rows; i++)
            {
                for (int j = 0; j < a.cols; j++)
                {
                    res.element[i * a.cols + j] = -res.element[i * a.cols + j];
                }
            }
            return res;
        }
        /// <summary>
        /// 矩陣相乘
        /// </summary>
        /// <param name="a">第一個矩陣</param>
        /// <param name="b">第二個矩陣,這個矩陣的行要與第一個矩陣的列相等</param>
        /// <returns>返回相乘後的一個新的矩陣</returns>
        public static GMatrix operator *(GMatrix a, GMatrix b)
        {
            if (a.cols == b.rows)
            {
                double[,] res = new double[a.rows, b.cols];
                for (int i = 0; i < a.rows; i++)
                {
                    for (int j = 0; j < b.cols; j++)
                    {
                        for (int k = 0; k < a.cols; k++)
                        {
                            res[i, j] += a[i, k] * b[k, j];
                        }
                    }
                }
                return new GMatrix(res);
            }
            else
            {
                throw new Exception("兩個矩陣行和列不等");
            }
        }
        /// <summary>
        /// 矩陣與數相乘
        /// </summary>
        /// <param name="a">第一個矩陣</param>
        /// <param name="num">一個實數</param>
        /// <returns>返回相乘後的新的矩陣</returns>
        public static GMatrix operator *(GMatrix a, double num)
        {
            GMatrix res = a;
            for (int i = 0; i < a.rows; i++)
            {
                for (int j = 0; j < a.cols; j++)
                {
                    res.element[i * a.cols + j] *= num;
                }
            }
            return res;
        }
        /// <summary>
        /// 矩陣轉置
        /// </summary>
        /// <returns>返回當前矩陣轉置後的新矩陣</returns>
        public GMatrix Transpose()
        {
            double[,] res = new double[cols, rows];
            {
                for (int i = 0; i < cols; i++)
                {
                    for (int j = 0; j < rows; j++)
                    {
                        res[i, j] = this[j, i];
                    }
                }
            }
            return new GMatrix(res);
        }
        /// <summary>
        /// 矩陣求逆
        /// </summary>
        /// <returns>返回求逆後的新的矩陣</returns>
        public GMatrix Inverse()
        {
            //最後原始矩陣並不變，所以需要深拷貝一份
            GMatrix _thisCopy = this.DeepCopy();
            if (cols == rows && this.Determinant() != 0)
            {
                //初始化一個同等大小的單位陣
                GMatrix res = _thisCopy.EGMatrix();
                for (int i = 0; i < rows; i++)
                {
                    //首先找到第i列的絕對值最大的數，並將該行和第i行互換
                    int rowMax = i;
                    double max = Math.Abs(_thisCopy[i, i]);
                    for (int j = i; j < rows; j++)
                    {
                        if (Math.Abs(_thisCopy[j, i]) > max)
                        {
                            rowMax = j;
                            max = Math.Abs(_thisCopy[j, i]);
                        }
                    }
                    //將第i行和找到最大數那一行rowMax交換
                    if (rowMax != i)
                    {
                        _thisCopy.Exchange(i, rowMax);
                        res.Exchange(i, rowMax);

                    }
                    //將第i行做初等行變換，將第一個非0元素化爲1
                    double r = 1.0 / _thisCopy[i, i];
                    _thisCopy.Exchange(i, -1, r);
                    res.Exchange(i, -1, r);
                    //消元
                    for (int j = 0; j < rows; j++)
                    {
                        //到本行後跳過
                        if (j == i)
                            continue;
                        else
                        {
                            r = -_thisCopy[j, i];
                            _thisCopy.Exchange(i, j, r);
                            res.Exchange(i, j, r);
                        }
                    }
                }
                return res;
            }
            else
            {
                throw new Exception("矩陣不是方陣無法求逆");
            }
        }
        #region 重載比較運算符
        public static bool operator <(GMatrix a, GMatrix b)
        {
            bool issmall = true;
            for (int i = 0; i < a.Rows; i++)
            {
                for (int j = 0; j < a.Cols; j++)
                {
                    if (a[i, j] >= b[i, j]) issmall = false;
                }
            }
            return issmall;
        }
        public static bool operator >(GMatrix a, GMatrix b)
        {
            bool issmall = true;
            for (int i = 0; i < a.Rows; i++)
            {
                for (int j = 0; j < a.Cols; j++)
                {
                    if (a[i, j] <= b[i, j]) issmall = false;
                }
            }
            return issmall;
        }
        public static bool operator <=(GMatrix a, GMatrix b)
        {
            bool issmall = true;
            for (int i = 0; i < a.Rows; i++)
            {
                for (int j = 0; j < a.Cols; j++)
                {
                    if (a[i, j] > b[i, j]) issmall = false;
                }
            }
            return issmall;
        }
        public static bool operator >=(GMatrix a, GMatrix b)
        {
            bool issmall = true;
            for (int i = 0; i < a.Rows; i++)
            {
                for (int j = 0; j < a.Cols; j++)
                {
                    if (a[i, j] < b[i, j]) issmall = false;
                }
            }
            return issmall;
        }
        public static bool operator !=(GMatrix a, GMatrix b)
        {
            bool issmall = true;
            issmall = ReferenceEquals(a, b);
            if (issmall) return issmall;
            for (int i = 0; i < a.Rows; i++)
            {
                for (int j = 0; j < a.Cols; j++)
                {
                    if (a[i, j] == b[i, j]) issmall = false;
                }
            }
            return issmall;
        }
        public static bool operator ==(GMatrix a, GMatrix b)
        {
            bool issmall = true;
            issmall = ReferenceEquals(a, b);
            if (issmall) return issmall;
            for (int i = 0; i < a.Rows; i++)
            {
                for (int j = 0; j < a.Cols; j++)
                {
                    if (a[i, j] != b[i, j]) issmall = false;
                }
            }
            return issmall;
        }
        public override bool Equals(object obj)
        {
            GMatrix b = obj as GMatrix;
            return this == b;
        }
        public override int GetHashCode()
        {
            return base.GetHashCode();
        }
        #endregion
        public double Determinant()
        {
            if (cols == rows)
            {
                GMatrix _thisCopy = this.DeepCopy();
                //行列式每次交換行，都需要乘以-1
                double res = 1;
                for (int i = 0; i < rows; i++)
                {
                    //首先找到第i列的絕對值最大的數
                    int rowMax = i;
                    double max = Math.Abs(_thisCopy[i, i]);
                    for (int j = i; j < rows; j++)
                    {
                        if (Math.Abs(_thisCopy[j, i]) > max)
                        {
                            rowMax = j;
                            max = Math.Abs(_thisCopy[j, i]);
                        }
                    }
                    //將第i行和找到最大數那一行rowMax交換,同時將單位陣做相同初等變換
                    if (rowMax != i)
                    {
                        _thisCopy.Exchange(i, rowMax);
                        res *= -1;
                    }
                    //消元
                    for (int j = i + 1; j < rows; j++)
                    {
                        double r = -_thisCopy[j, i] / _thisCopy[i, i];
                        _thisCopy.Exchange(i, j, r);
                    }
                }
                //計算對角線乘積
                for (int i = 0; i < rows; i++)
                {
                    res *= _thisCopy[i, i];
                }
                return res;
            }
            else
            {
                throw new Exception("不是行列式");
            }
        }
        #endregion
        #region 初等變換
        /// <summary>
        /// 初等變換：交換第r1和第r2行
        /// </summary>
        /// <param name="r1">第r1行</param>
        /// <param name="r2">第r2行</param>
        /// <returns>返回交換兩行後的新的矩陣</returns>
        public GMatrix Exchange(int r1, int r2)
        {
            if (Math.Min(r2, r1) >= 0 && Math.Max(r1, r2) < rows)
            {
                for (int j = 0; j < cols; j++)
                {
                    double temp = this[r1, j];
                    this[r1, j] = this[r2, j];
                    this[r2, j] = temp;
                }
                return this;
            }
            else
            {
                throw new Exception("超出索引");
            }
        }
        /// <summary>
        /// 初等變換：將r1行乘以某個數加到r2行
        /// </summary>
        /// <param name="r1">第r1行乘以num</param>
        /// <param name="r2">加到第r2行，若第r2行爲負，則直接將r1乘以num並返回</param>
        /// <param name="num">某行放大的倍數</param>
        /// <returns></returns>
        public GMatrix Exchange(int r1, int r2, double num)
        {
            if (Math.Min(r2, r1) >= 0 && Math.Max(r1, r2) < rows)
            {
                for (int j = 0; j < cols; j++)
                {
                    this[r2, j] += this[r1, j] * num;
                }
                return this;
            }
            else if (r2 < 0)
            {
                for (int j = 0; j < cols; j++)
                {
                    this[r1, j] *= num;
                }
                return this;
            }
            else
            {
                throw new Exception("超出索引");
            }
        }
        /// <summary>
        /// 得到一個同等大小的單位矩陣
        /// </summary>
        /// <returns>返回一個同等大小的單位矩陣</returns>
        public GMatrix EGMatrix()
        {
            if (rows == cols)
            {
                double[,] res = new double[rows, cols];
                for (int i = 0; i < rows; i++)
                {
                    for (int j = 0; j < cols; j++)
                    {
                        if (i == j)
                            res[i, j] = 1;
                        else
                            res[i, j] = 0;
                    }
                }
                return new GMatrix(res);
            }
            else
                throw new Exception("不是方陣，無法得到單位矩陣");
        }
        #endregion
        /// <summary>
        /// 深拷貝，僅僅將值拷貝給一個新的對象
        /// </summary>
        /// <returns>返回深拷貝後的新對象</returns>
        public GMatrix DeepCopy()
        {
            double[,] ele = new double[rows, cols];
            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    ele[i, j] = this[i, j];
                }
            }
            return new GMatrix(ele);
        }

        public override string ToString()
        {
            string str = "";
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Cols; j++)
                {
                    str += this[i, j].ToString();
                    if (j != Cols - 1)
                        str += " ";
                    else if (i != Rows - 1)
                        str += Environment.NewLine;
                }
            }
            return str;
        }
    }
}