几个WSG84转UTM坐标的函数python与cpp实现

WSG84转UTM坐标函数总结：

# https://github.com/Turbo87/utm
import numpy as np
def from_latlon(latitude, longitude, force_zone_number=None, force_zone_letter=None):
    """This function converts Latitude and Longitude to UTM coordinate
        Parameters
        ----------
        latitude: float or NumPy array
            Latitude between 80 deg S and 84 deg N, e.g. (-80.0 to 84.0)
        longitude: float or NumPy array
            Longitude between 180 deg W and 180 deg E, e.g. (-180.0 to 180.0).
        force_zone_number: int
            Zone number is represented by global map numbers of an UTM zone
            numbers map. You may force conversion to be included within one
            UTM zone number.  For more information see utmzones [1]_
        force_zone_letter: str
            You may force conversion to be included within one UTM zone
            letter.  For more information see utmzones [1]_
        Returns
        -------
        easting: float or NumPy array
            Easting value of UTM coordinates
        northing: float or NumPy array
            Northing value of UTM coordinates
        zone_number: int
            Zone number is represented by global map numbers of a UTM zone
            numbers map. More information see utmzones [1]_
        zone_letter: str
            Zone letter is represented by a string value. UTM zone designators
            can be accessed in [1]_
       .. _[1]: http://www.jaworski.ca/utmzones.htm
    """
    
    def latlon_to_zone_number(latitude, longitude):
        # If the input is a numpy array, just use the first element
        # User responsibility to make sure that all points are in one zone
        if isinstance(latitude, np.ndarray):
            latitude = latitude.flat[0]
        if isinstance(longitude, np.ndarray):
            longitude = longitude.flat[0]
        if 56 <= latitude < 64 and 3 <= longitude < 12:
            return 32
        if 72 <= latitude <= 84 and longitude >= 0:
            if longitude < 9:
                return 31
            elif longitude < 21:
                return 33
            elif longitude < 33:
                return 35
            elif longitude < 42:
                return 37
        return int((longitude + 180) / 6) + 1
    
    def latitude_to_zone_letter(latitude):
        if -80 <= latitude <= 84:
            return ZONE_LETTERS[int(latitude / 8) + 10]
        return "Z"
    
    def zone_number_to_central_longitude(zone_number):
        return zone_number * 6 - 183
    
    def mod_angle(value):
        """Returns angle in radians to be between -pi and pi"""
        return (value + np.pi) % (2 * np.pi) - np.pi
    def mixed_signs(x):
        return np.min(x) < 0 and np.max(x) >= 0
    def negative(x):
        return np.max(x) < 0
    
    K0 = 0.9996
 
    E = 0.00669438
    E2 = E * E
    E3 = E2 * E
    E_P2 = E / (1 - E)
 
    SQRT_E = np.sqrt(1 - E)
    _E = (1 - SQRT_E) / (1 + SQRT_E)
    _E2 = _E * _E
    _E3 = _E2 * _E
    _E4 = _E3 * _E
    _E5 = _E4 * _E
 
    M1 = (1 - E / 4 - 3 * E2 / 64 - 5 * E3 / 256)
    M2 = (3 * E / 8 + 3 * E2 / 32 + 45 * E3 / 1024)
    M3 = (15 * E2 / 256 + 45 * E3 / 1024)
    M4 = (35 * E3 / 3072)
 
    P2 = (3 / 2 * _E - 27 / 32 * _E3 + 269 / 512 * _E5)
    P3 = (21 / 16 * _E2 - 55 / 32 * _E4)
    P4 = (151 / 96 * _E3 - 417 / 128 * _E5)
    P5 = (1097 / 512 * _E4)
 
    R = 6378137
 
    ZONE_LETTERS = "CDEFGHJKLMNPQRSTUVWXX"
    
    lat_rad = np.radians(latitude)
    lat_sin = np.sin(lat_rad)
    lat_cos = np.cos(lat_rad)
 
    lat_tan = lat_sin / lat_cos
    lat_tan2 = lat_tan * lat_tan
    lat_tan4 = lat_tan2 * lat_tan2
 
    if force_zone_number is None:
        zone_number = latlon_to_zone_number(latitude, longitude)
    else:
        zone_number = force_zone_number
 
    if force_zone_letter is None:
        zone_letter = latitude_to_zone_letter(latitude)
    else:
        zone_letter = force_zone_letter
 
    lon_rad = np.radians(longitude)
    central_lon = zone_number_to_central_longitude(zone_number)
    central_lon_rad = np.radians(central_lon)
 
    n = R / np.sqrt(1 - E * lat_sin**2)
    c = E_P2 * lat_cos**2
 
    a = lat_cos * mod_angle(lon_rad - central_lon_rad)
    a2 = a * a
    a3 = a2 * a
    a4 = a3 * a
    a5 = a4 * a
    a6 = a5 * a
 
    m = R * (M1 * lat_rad -
             M2 * np.sin(2 * lat_rad) +
             M3 * np.sin(4 * lat_rad) -
             M4 * np.sin(6 * lat_rad))
 
    easting = K0 * n * (a +
                        a3 / 6 * (1 - lat_tan2 + c) +
                        a5 / 120 * (5 - 18 * lat_tan2 + lat_tan4 + 72 * c - 58 * E_P2)) + 500000
 
    northing = K0 * (m + n * lat_tan * (a2 / 2 +
                                        a4 / 24 * (5 - lat_tan2 + 9 * c + 4 * c**2) +
                                        a6 / 720 * (61 - 58 * lat_tan2 + lat_tan4 + 600 * c - 330 * E_P2)))
 
    if mixed_signs(latitude):
        raise ValueError("latitudes must all have the same sign")
    elif negative(latitude):
        northing += 10000000
 
    return easting, northing, zone_number, zone_letter

2.手写的坐标转换

#!/usr/bin/python3
 
__author__ = 'ISmileLi'
 
from osgeo import gdal, ogr, osr
from pyproj import Transformer
 
'''
osgeo底层坐标转换使用的库还是proj,下面函数中的espg值需要根据自己的需求进行修改，
下文测试使用的是wgs84与中国区高斯-克吕格EPSG码为21460区的转换
'''
 
def lonLat_to_gauss(lon, lat, from_epsg=4326, to_epsg=21460):
    '''
    经纬度转高斯
    :param lon:
    :param lat:
    :param from_epsg:
    :param to_EPSG:
    :return:
    '''
 
    from_spa = osr.SpatialReference()
    '''
    gdal版本大于3.0以后必须设置转换策略才能正确显示结果，否则结果将会输出'inf'
    可以了解官方的这个issue说明：https://github.com/OSGeo/gdal/issues/1546
    '''
    if int(gdal.__version__[0]) >= 3:
        from_spa.SetAxisMappingStrategy(osr.OAMS_TRADITIONAL_GIS_ORDER)
    from_spa.ImportFromEPSG(from_epsg)
    to_spa = osr.SpatialReference()
    to_spa.ImportFromEPSG(to_epsg)
    coord_trans = osr.CoordinateTransformation(from_spa, to_spa)
 
    t = coord_trans.TransformPoint(lon, lat)
    return t[0], t[1]
 
def gauss_to_lonLat(x, y, from_epsg=21460, to_epsg=4326):
    '''
    高斯转经纬度
    :param x:
    :param y:
    :param from_epsg:
    :param to_EPSG:
    :return:
    '''
 
    from_spa = osr.SpatialReference()
    #if int(gdal.__version__[0]) >= 3:
        #from_spa.SetAxisMappingStrategy(osr.OAMS_TRADITIONAL_GIS_ORDER)
    from_spa.ImportFromEPSG(from_epsg)
    to_spa = osr.SpatialReference()
    to_spa.ImportFromEPSG(to_epsg)
    coord_trans = osr.CoordinateTransformation(from_spa, to_spa)
 
    t = coord_trans.TransformPoint(x, y)
    return t[0], t[1]
 
 
def lonLat_to_gauss_proj(lon, lat, from_epsg="EPSG:4326", to_epsg="EPSG:21460"):
    '''
    使用proj库经纬度转高斯
    :param lon:
    :param lat:
    :param from_epsg:
    :param to_epsg:
    :return:
    '''
    transfromer = Transformer.from_crs(from_epsg, to_epsg,always_xy=True)  # WGS-84对应码->EPSG:4326, 中国高斯对应码：EPSG:21460
    x, y = transfromer.transform(lon, lat)
    print('lonLat_to_gauss_proj x, y:',x, y)
    return x, y
 
def gauss_to_lonLat_proj(x, y, from_epsg="EPSG:21460", to_epsg="EPSG:4326"):
    '''
    使用proj库高斯转经纬度
    :param x:
    :param y:
    :param from_epsg:
    :param to_epsg:
    :return:
    '''
    transfromer = Transformer.from_crs(from_epsg, to_epsg, always_xy=True)  # WGS-84对应码->EPSG:4326, 中国高斯对应码：EPSG:21460
    lon, lat = transfromer.transform(x, y)
    print('lonLat_to_gauss_proj lon, lat:', lon, lat)
    return lon, lat
 
if __name__ == '__main__':
    lon = 116.2446370442708300
    lat = 40.0670713975694400
    x, y = lonLat_to_gauss(lon, lat)
    print('x, y: ', x, y)
    lat_t, lon_t = gauss_to_lonLat(x, y)
    print('lon_t, lat_t: ', lon_t, lat_t)
 
    '''
    这里要注意pyproj的转换会交换x/y返回，可以对比osgeo使用打印结果看出来，
    详细了解可以参考官网文档：https://pyproj4.github.io/pyproj/stable/api/transformer.html
    '''
    lon_t = 116.2446370442708300
    lat_t = 40.0670713975694400
    x_t, y_t = lonLat_to_gauss_proj(lon_t, lat_t)
    gauss_to_lonLat_proj(x_t, y_t)

def s84_to_utm(latitude,longitude):
    wgs84 = pyproj.CRS.from_string('EPSG:4326') # 使用WGS84 CRS
    utm = pyproj.CRS.from_string('EPSG:32650') # 使用UTM Zone 50 CRS

    transformer = pyproj.Transformer.from_crs(wgs84, utm, always_xy=True)

    # 经纬度转UTM坐标
    utm_x, utm_y = transformer.transform(longitude, latitude)

    # print("UTM坐标:", "[",utm_x,",",utm_y,"]")
    return utm_x,utm_y

这一个是直接用库，所以参数是需要自己进行修改的。主要是utm的参数。

接下来就好好验证一下吧。
都是：

好，如果是在cpp里面则：

//utm.h
#include <cmath>
#include<iostream>
#include <algorithm>


struct UTMCoordinates {
    double easting;
    double northing;
    int zone_number;
    char zone_letter;
};

UTMCoordinates from_latlon(double latitude, double longitude, int force_zone_number = -1, char force_zone_letter = 'Z') {
    auto latlon_to_zone_number = [](double latitude, double longitude) {
        if (56 <= latitude && latitude < 64 && 3 <= longitude && longitude < 12) {
            return 32;
        }
        if (72 <= latitude && latitude <= 84 && longitude >= 0) {
            if (longitude < 9) {
                return 31;
            }
            else if (longitude < 21) {
                return 33;
            }
            else if (longitude < 33) {
                return 35;
            }
            else if (longitude < 42) {
                return 37;
            }
        }
        return static_cast<int>((longitude + 180) / 6) + 1;
    };

    auto latitude_to_zone_letter = [](double latitude) {
        if (-80 <= latitude && latitude <= 84) {
            const char ZONE_LETTERS[] = "CDEFGHJKLMNPQRSTUVWXX";
            return ZONE_LETTERS[static_cast<int>(latitude / 8) + 10];
        }
        return 'Z';
    };

    auto zone_number_to_central_longitude = [](int zone_number) {
        return zone_number * 6 - 183;
    };

    auto mod_angle = [](double value) {
        return std::fmod(value + M_PI, 2 * M_PI) - M_PI;
    };

    auto mixed_signs = [](const double* x) {
        return (std::min_element(x, x + 2) < 0) && (std::max_element(x, x + 2) >= 0);
    };

    auto negative = [](const double* x) {
        return std::max_element(x, x + 2) < 0;
    };

    const double K0 = 0.9996;
    const double E = 0.00669438;
    const double E2 = E * E;
    const double E3 = E2 * E;
    const double E_P2 = E / (1 - E);
    const double SQRT_E = std::sqrt(1 - E);
    const double _E = (1 - SQRT_E) / (1 + SQRT_E);
    const double _E2 = _E * _E;
    const double _E3 = _E2 * _E;
    const double _E4 = _E3 * _E;
    const double _E5 = _E4 * _E;
    const double M1 = (1 - E / 4 - 3 * E2 / 64 - 5 * E3 / 256);
    const double M2 = (3 * E / 8 + 3 * E2 / 32 + 45 * E3 / 1024);
    const double M3 = (15 * E2 / 256 + 45 * E3 / 1024);
    const double M4 = (35 * E3 / 3072);
    const double P2 = (3 / 2 * _E - 27 / 32 * _E3 + 269 / 512 * _E5);
    const double P3 = (21 / 16 * _E2 - 55 / 32 * _E4);
    const double P4 = (151 / 96 * _E3 - 417 / 128 * _E5);
    const double P5 = (1097 / 512 * _E4);
    const double R = 6378137;
    const char ZONE_LETTERS[] = "CDEFGHJKLMNPQRSTUVWXX";

    double lat_rad = latitude * M_PI / 180;
    double lat_sin = std::sin(lat_rad);
    double lat_cos = std::cos(lat_rad);
    double lat_tan = lat_sin / lat_cos;
    double lat_tan2 = lat_tan * lat_tan;
    double lat_tan4 = lat_tan2 * lat_tan2;

    int zone_number, central_lon;
    char zone_letter;

    if (force_zone_number == -1) {
        zone_number = latlon_to_zone_number(latitude, longitude);
    }
    else {
        zone_number = force_zone_number;
    }

    if (force_zone_letter == 'Z') {
        zone_letter = latitude_to_zone_letter(latitude);
    }
    else {
        zone_letter = force_zone_letter;
    }

    central_lon = zone_number_to_central_longitude(zone_number);

    double lon_rad = longitude * M_PI / 180;
    double central_lon_rad = central_lon * M_PI / 180;

    double n = R / std::sqrt(1 - E * lat_sin * lat_sin);
    double c = E_P2 * lat_cos * lat_cos;

    double a = lat_cos * (lon_rad - central_lon_rad);
    double a2 = a * a;
    double a3 = a2 * a;
    double a4 = a3 * a;
    double a5 = a4 * a;
    double a6 = a5 * a;

    double m = R * (M1 * lat_rad - M2 * std::sin(2 * lat_rad) + M3 * std::sin(4 * lat_rad) - M4 * std::sin(6 * lat_rad));
    double x = K0 * n * (a + a3 / 6 * (1 - lat_tan2 + c) + a5 / 120 * (5 - 18 * lat_tan2 + lat_tan4 + 72 * c - 58 * E_P2))
        + 500000;
    double y = K0 * (m + n * lat_tan * (a2 / 2 + a4 / 24 * (5 - lat_tan2 + 9 * c + 4 * c * c) + a6 / 720 * (61 - 58 * lat_tan2 + lat_tan4 + 600 * c - 330 * E_P2)));

    if (mixed_signs(&lat_rad)) {
        y = -y;
    }

    if (negative(&lat_rad)) {
        zone_letter = 'A' + (zone_letter - 'N');
    }

    return { x, y, zone_number, zone_letter };
}

做一下验证：

#include<iostream>
#include "utm.h"
#include<iomanip>  //注意c++中只能显示6位数字，要想多显示那就要yogasetprecision

using namespace std;

int main() {
    double buf_pos_lat = 39.14144348489;
    double buf_pos_lon = 117.77482815479;
    double buf_vel = 1;
    double buf_angle_heading = -116.74334284700001;
    int force_zone_number = -1;
    char force_zone_letter = 'Z';
    UTMCoordinates utm_coords = from_latlon(buf_pos_lat, buf_pos_lon, force_zone_number, force_zone_letter);
    std::cout << "Easting: " <<std::setprecision(16)<<utm_coords.easting << std::endl;
    std::cout << "Northing: " << utm_coords.northing << std::endl;
    std::cout << "Zone number: " << utm_coords.zone_number << std::endl;
    std::cout << "Zone letter: " << utm_coords.zone_letter << std::endl;

    return 0;
}