经纬度坐标转平面直角坐标——高斯投影python/c#实现

高斯投影正算公式来自《大地测量学第二版》，作者孔祥元

公式中各个参数的含义，详见博客：https://blog.csdn.net/du12300/article/details/109307386

实现代码如下，为了避免出现负的横坐标，可在Y上加上500000m

import numpy as np
import pandas as pd
import math


class Point:
    def __init__(self, type=0):
        self.B = 0
        self.L = 0
        self.H = 0

        ##### 椭球参数
        # CGCS2000
        if type == 1:
            self.a = 6378137
            self.f = 1 / 298.257222101

        ## 西安80
        elif type == 2:
            self.a = 6378140
            self.f = 1 / 298.257

        ## 北京54
        elif type == 3:
            self.a = 6378245
            self.f = 1 / 298.3

        ## WGS-84
        else:
            self.a = 6378137
            self.f = 1 / 298.257223563

        self.b = self.a * (1 - self.f)
        self.e = math.sqrt(self.a * self.a - self.b * self.b) / self.a  ##第一偏心率
        self.e2 = math.sqrt(self.a * self.a - self.b * self.b) / self.b ##第二偏心率
        self.d2r = math.pi / 180

    def SetBLH_DMS(self, degree_L=0.0, minutes_L=0.0, second_L=0.0, degree_B=0.0, minutes_B=0.0, second_B=0.0, H_=0.0):
        ## 度分秒化为 度.度
        self.L = (degree_L + minutes_L / 60.0 + second_L / 3600.0)
        self.B = (degree_B + minutes_B / 60.0 + second_B / 3600.0)
        self.H = H_

    def SetBLH_D(self, B, L, H=0):
        ## 经纬度格式是 度.度
        self.L = L
        self.B = B
        self.H = H

    def GaussProjection(self):
        L0 = 114    ##中央子午线，这里是3度带的取法，6度带注意修改这里
        rho = 180 / math.pi  # 单位是度.度
        l = (self.L - L0) / rho     ## 公式中的参数 l

        cB = math.cos(self.B * self.d2r)
        sB = math.sin(self.B * self.d2r)
        self.N = self.a / math.sqrt(1 - self.e * self.e * sB * sB)  ## 卯酉圈曲率半径
        self.t = math.tan(self.B * self.d2r)        ##tanB 对应公式中的参数t
        self.eta = self.e2 * cB     ## 对应公式中的参数 eta

        m0 = self.a * (1 - self.e * self.e)
        m2 = 3.0/2.0 * self.e * self.e * m0
        m4 = 5.0/4.0 * self.e * self.e * m2
        m6 = 7.0/6.0 * self.e * self.e * m4
        m8 = 9.0/8.0 * self.e * self.e * m6

        a0 = m0 + 1.0/2.0 * m2 + 3.0/8.0 * m4 + 5.0/16.0 * m6 + 35.0/128.0 * m8
        a2 = 1.0/2.0 * m2 + 1.0/2.0 * m4 + 15.0/32.0 * m6 + 7.0/16.0 * m8
        a4 = 1.0/8.0 * m4 + 3.0/16.0 * m6 + 7.0/32.0 * m8
        a6 = 1.0/32.0 * m6 + 1.0/16.0 * m8
        a8 = 1.0/128.0 * m8

        s2b = math.sin(self.B * self.d2r * 2)
        s4b = math.sin(self.B * self.d2r * 4)
        s6b = math.sin(self.B * self.d2r * 6)
        s8b = math.sin(self.B * self.d2r * 8)

        ## X为自赤道量起的子午线弧长
        self.X = a0 * (self.B * self.d2r) - 1.0/2.0 * a2 * s2b + 1.0/4.0 * a4 * s4b - 1.0/6.0 * a6 * s6b + 1.0/8.0 * a8 * s8b

        ## 坐标X
        self.xs = self.X + self.N / 2 * self.t * cB * cB * l * l + self.N / 24 * self.t * (5 - self.t * self.t + 9 * math.pow(self.eta, 2) + 4 * math.pow(self.eta, 4)) * math.pow(cB, 4) * math.pow(l, 4) \
                  + self.N / 720 * self.t * (61 - 58 * self.t * self.t + math.pow(self.t, 4)) * math.pow(cB, 6) * math.pow(l, 6)

        ## 坐标Y
        self.ys = self.N * cB * l + self.N / 6 * (1 - self.t * self.t + self.eta * self.eta) * math.pow(cB, 3) * math.pow(l, 3) \
                  + self.N / 120 * (5 - 18 * self.t * self.t + math.pow(self.t, 4) + 14 * math.pow(self.eta, 2) - 58 * self.eta * self.eta * self.t * self.t) * math.pow(cB, 5) * math.pow(l, 5) + 500000

        self.hs = self.H

        self.xyh_vector = [[self.xs],
                           [self.ys],
                           [self.hs]]
        self.xyh_vector = np.array(self.xyh_vector)


if __name__ == "__main__":
    rawData = pd.read_csv(r"2020.csv")
    numberOfPoint = 10  # 选取多少个点计算
    for i in range(numberOfPoint):
        L_txt = str(rawData.iat[i, 4])
        L_degree = float(L_txt[:3])
        L_minute = float(L_txt[4:6])
        L_second = float(L_txt[6:8] + "." + L_txt[8:])
        B_txt = str(rawData.iat[i, 3])
        B_degree = float(B_txt[:3])
        B_minute = float(B_txt[3:5])
        B_second = float(B_txt[5:7] + "." + B_txt[7:])
        H = float(rawData.iat[i, 5])
        p = Point(type=1)
        p.SetBLH_DMS(L_degree, L_minute, L_second, B_degree, B_minute, B_second, H)
        p.GaussProjection()

        print(p.xyh_vector[0], p.xyh_vector[1])
        print(" ")

C#实现，C#矩阵运算库

using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;

namespace BLHToLocal
{
    public class Point
    {
        //纬度 经度 高度
        public double B, L, H;
        // 长半轴 短半轴 扁率 第一偏心率 第二偏心率
        public double a, b, f, e, e2, d2r = Math.PI / 180;
        //中央子午线
        public double L0 = 114;
        public double rho = 180 / Math.PI;
        public double l;
        //平面坐标系X1 Y1 H
        public double xs, ys, hs;

        // 椭球参数设置
        public void EllipsoidParam(string type = "WGS-84", double L0_ = 114)
        {
            // CGCS2000 椭球参数
            if (type == "CGCS2000")
            {
                this.a = 6378137;
                this.f = 1 / 298.257222101;
                this.L0 = L0_;
            }

            // 西安 80
            else if (type == "西安80")
            {
                this.a = 6378140;
                this.f = 1 / 298.257;
                this.L0 = L0_;
            }

            // 北京 54
            else if (type == "北京54")
            {
                this.a = 6378245;
                this.f = 1 / 298.3;
                this.L0 = L0_;
            }

            // WGS-84 
            else
            {
                this.a = 6378137;
                this.f = 1 / 298.257223563;
                this.L0 = L0_;
            }

            this.b = this.a * (1 - this.f);
            this.e = Math.Sqrt(this.a * this.a - this.b * this.b) / this.a;  //第一偏心率
            this.e2 = Math.Sqrt(this.a * this.a - this.b * this.b) / this.b;  //第二偏心率
        }

        // 以度分秒的形式输入经纬度
        public void SetBLH_DMS(double degree_L=0.0, double minutes_L=0.0, double second_L=0.0, double degree_B=0.0, double minutes_B=0.0, double second_B=0.0, double H_=0.0)
        {
            this.L = (degree_L + minutes_L / 60.0 + second_L / 3600.0);
            this.B = (degree_B + minutes_B / 60.0 + second_B / 3600.0);
            this.H = H_;
        }

        //以 度.度 的形式输入经纬度
        public void SetBLH_Degree(double B_, double L_, double H_=0.0)
        {
            this.L = L_;
            this.B = B_;
            this.H = H_;
        }

        //高斯投影
        public void GaussProjection(string type = "WGS-84", double L0_ = 114)
        {
            this.EllipsoidParam(type, L0_);

            double l = (this.L - this.L0) / this.rho;

            double cB = Math.Cos(this.B * this.d2r);
            double sB = Math.Sin(this.B * this.d2r);
            double s2b = Math.Sin(this.B * this.d2r * 2);
            double s4b = Math.Sin(this.B * this.d2r * 4);
            double s6b = Math.Sin(this.B * this.d2r * 6);
            double s8b = Math.Sin(this.B * this.d2r * 8);

            double N = this.a / Math.Sqrt(1 - this.e * this.e * sB * sB);       // 卯酉圈曲率半径
            double t = Math.Tan(this.B * this.d2r);
            double eta = this.e2 * cB;

            double m0 = this.a * (1 - this.e * this.e);
            double m2 = 3.0 / 2.0 * this.e * this.e * m0;
            double m4 = 5.0 / 4.0 * this.e * this.e * m2;
            double m6 = 7.0 / 6.0 * this.e * this.e * m4;
            double m8 = 9.0 / 8.0 * this.e * this.e * m6;

            double a0 = m0 + 1.0 / 2.0 * m2 + 3.0 / 8.0 * m4 + 5.0 / 16.0 * m6 + 35.0 / 128.0 * m8;
            double a2 = 1.0 / 2.0 * m2 + 1.0 / 2.0 * m4 + 15.0 / 32.0 * m6 + 7.0 / 16.0 * m8;
            double a4 = 1.0 / 8.0 * m4 + 3.0 / 16.0 * m6 + 7.0 / 32.0 * m8;
            double a6 = 1.0 / 32.0 * m6 + 1.0 / 16.0 * m8;
            double a8 = 1.0 / 128.0 * m8;

            // X1为自赤道量起的子午线弧长
            double X1 = a0 * (this.B * this.d2r) - 1.0 / 2.0 * a2 * s2b + 1.0 / 4.0 * a4 * s4b - 1.0 / 6.0 * a6 * s6b + 1.0 / 8.0 * a8 * s8b;

            this.xs = X1 + N / 2 * t * cB * cB * l * l + N / 24 * t * (5 - t * t + 9 * Math.Pow(eta, 2) + 4 * Math.Pow(eta, 4)) * Math.Pow(cB, 4) * Math.Pow(l, 4)
                  + N / 720 * t * (61 - 58 * t * t + Math.Pow(t, 4)) * Math.Pow(cB, 6) * Math.Pow(l, 6);

            this.ys = N * cB * l + N / 6 * (1 - t * t + eta * eta) * Math.Pow(cB, 3) * Math.Pow(l, 3)
                + N / 120 * (5 - 18 * t * t + Math.Pow(t, 4) + 14 * Math.Pow(eta, 2) - 58 * eta * eta * t * t) * Math.Pow(cB, 5) * Math.Pow(l, 5) + 500000;

            this.hs = this.H;
        }

        // 添加偏差补偿
        public void DeviationCompensate(double x_devi, double y_devi)
        {
            this.xs += x_devi;
            this.ys += y_devi;
        }

        //坐标转换
        public double[] CoordinateTranfer(NanaParam nnp)
        {
            Matrix R = DenseMatrix.OfArray(new double[,] {
                                                    { 1, nnp.omigaZ, -nnp.omigaY },
                                                    { -nnp.omigaZ, 1, nnp.omigaX },
                                                    { nnp.omigaY, -nnp.omigaX, 1 } });

            var deltaMove = new DenseVector(new[] { nnp.deltaX, nnp.deltaY, nnp.deltaZ });
            var coor1 = new DenseVector(new[] { this.xs, this.ys, this.hs });

            var coor2 = (1 + nnp.m) * R * coor1 + deltaMove;

            double[] result = new double[3] { coor2[0], coor2[1], coor2[2] };

            return result;
        }
    }

    public class NanaParam
    {
        //X1 Y1 Z1 表示坐标系1中坐标    X2 Y2 Z2 表示坐标系2中坐标
        public List X1 = new List();
        public List Y1 = new List();
        public List Z1 = new List();
        public List X2 = new List();
        public List Y2 = new List();
        public List Z2 = new List();

        //七参数 三个平移参数 三个旋转参数 一个尺度参数m
        public double deltaX, deltaY, deltaZ, omigaX, omigaY, omigaZ, m;

        public void Add(double x1, double y1, double z1, double x2, double y2, double z2)
        {
            this.X1.Add(x1);
            this.Y1.Add(y1);
            this.Z1.Add(z1);

            this.X2.Add(x2);
            this.Y2.Add(y2);
            this.Z2.Add(z2);
        }

        public void Solve()
        {
            var G = new DenseMatrix(X2.Count * 3, 7);
            var P = new DenseMatrix(X2.Count * 3, 1);

            for (int i = 0; i < X2.Count; i++)
            {
                G[3 * i, 0] = 1;
                G[3 * i, 1] = 0;
                G[3 * i, 2] = 0;
                G[3 * i, 3] = X1[i];
                G[3 * i, 4] = 0;
                G[3 * i, 5] = -Z1[i];
                G[3 * i, 6] = Y1[i];

                G[3 * i + 1, 0] = 0;
                G[3 * i + 1, 1] = 1;
                G[3 * i + 1, 2] = 0;
                G[3 * i + 1, 3] = Y1[i];
                G[3 * i + 1, 4] = Z1[i];
                G[3 * i + 1, 5] = 0;
                G[3 * i + 1, 6] = -X1[i];

                G[3 * i + 2, 0] = 0;
                G[3 * i + 2, 1] = 0;
                G[3 * i + 2, 2] = 1;
                G[3 * i + 2, 3] = Z1[i];
                G[3 * i + 2, 4] = -Y1[i];
                G[3 * i + 2, 5] = X1[i];
                G[3 * i + 2, 6] = 0;

                P[3 * i, 0] = X2[i];
                P[3 * i + 1, 0] = Y2[i];
                P[3 * i + 2, 0] = Z2[i];
            }
            var G_T = G.Transpose();
            var K = G_T * G;
            var X = K.Inverse() * (G_T * P);

            this.deltaX = X[0, 0];
            this.deltaY = X[1, 0];
            this.deltaZ = X[2, 0];
            this.m = X[3, 0] - 1;
            this.omigaX = X[4, 0] / X[3, 0];
            this.omigaY = X[5, 0] / X[3, 0];
            this.omigaZ = X[6, 0] / X[3, 0];
        }

        public void SetNanaParam(double deltaX_, double deltaY_, double deltaZ_, double omigaX_, double omigaY_, double omigaZ_, double m_)
        {
            this.deltaX = deltaX_;
            this.deltaY = deltaY_;
            this.deltaZ = deltaZ_;
            this.omigaX = omigaX_;
            this.omigaY = omigaY_;
            this.omigaZ = omigaZ_;
            this.m = m_;
        }
    }
}