Geodesic Calculations Checker

cartesian_to_lat_lon.py

#!/usr/bin/env python3

# Tested with python3.11, geographiclib==2.0, matplotlib==3.8.2, numpy==1.26.2
import math
import numpy as np
import csv
import argparse

def get_x_y_data(filename):
    # Read the first line of the CSV file
    print(f"Reading CSV data from {filename}")
    try:
        with open(filename) as csv_file:
            reader = csv.reader(csv_file)
            headings = next(reader)
    except:
        print(f"ERROR: Could not parse {filename}")
        print("Check file exists and is a valid CSV file")
        exit()
    valid_x_col_names = ["x", "x-position", "x-offset", "x-positions", "x-offsets", "x position", 
                         "x offset", "x positions", "x offsets", "offset x", "position x"]
    valid_y_col_names = ["y", "y-position", "y-offset", "y-position", "y-offset", "y position", 
                         "y offset", "y positions", "y offsets", "offset y", "position y"]
    x_col, y_col = None, None

    for index, heading in enumerate(headings):
        if heading.lower() in valid_x_col_names:
            x_col = (index, heading)
        if heading.lower() in valid_y_col_names:
            y_col = (index, heading)
    if x_col is None:
        print("ERROR: Can't find X Position heading. Valid headings are: ", end="")
        for name in valid_x_col_names:
            print(f"'{name}' ", end="")
        print()
        exit()
    else:
        print(f"Using X Position heading: '{x_col[1]}'")
    if y_col is None:
        print("ERROR: Can't find Y Position heading. Valid headings are: ", end="")
        for name in valid_y_col_names:
            print(f"'{name}' ", end="")
        print()
        exit()
    else:
        print(f"Using Y Position heading: '{y_col[1]}'")
    X = np.loadtxt(filename, delimiter=',', skiprows=1, usecols=x_col[0])
    Y = np.loadtxt(filename, delimiter=',', skiprows=1, usecols=y_col[0])
    if len(X) != len(Y):
        print("ERROR: Failed to parse CSV file")
        exit()
    return X, Y

def get_new_lat_long_haversine(lat, lon, X, Y):
    # https://www.movable-type.co.uk/scripts/latlong.html
    bearing = np.arctan2(X,Y)
    distance = np.sqrt(np.power(X, 2)+np.power(Y,2))
    lat1 = math.radians(lat)
    lon1 = math.radians(lon)
    R = 6371000 # Earth radius in m

    lat2 = np.arcsin(math.sin(lat1) * np.cos(distance/R) +
                        math.cos(lat1) * np.sin(distance/R) * np.cos(bearing))
    lon2 = lon1 + np.arctan2(np.sin(bearing) * np.sin(distance / R) * math.cos(lat1),
                            np.cos(distance/R) - math.sin(lat1) * np.sin(lat2))

    return np.column_stack((np.degrees(lat2), np.degrees(lon2)))

def get_new_lat_long_fast_estimate(lat1, lon1, X, Y):
    # https://github.com/mapbox/cheap-ruler/blob/master/index.js

    # Values that define WGS84 ellipsoid model of the Earth
    RE = 6378.137 # equatorial radius
    FE = 1 / 298.257223563 # flattening
    E2 = FE * (2.0 - FE)
    RAD = math.pi / 180.0

    m = RAD * RE * 1000.0
    coslat = math.cos(lat1 * RAD)
    w2 = 1 / (1 - E2 * (1 - coslat * coslat))
    w = math.sqrt(w2)

    # Multipliers for converting longitude and latitude degrees into distance
    kx = m * w * coslat # based on normal radius of curvature
    ky = m * w * w2 * (1 - E2) # based on meridonal radius of curvature

    lon2 = lon1 + X / kx
    lat2 = lat1 + Y / ky
    return np.column_stack((lat2, lon2))

def get_lat_lon_geographiclib(lat1, lon1, X, Y):
    # https://geographiclib.sourceforge.io/C++/doc/
    # Transform to latitude and longitude
    bearing = np.degrees(np.arctan2(X,Y))
    distance = np.sqrt(np.power(X, 2)+np.power(Y,2))
    lat_lon_data = np.array(
        [_get_geographiclib_direct(lat1, lon1, azi1, s12) for azi1, s12 in zip(bearing, distance)])
    return lat_lon_data

def _get_geographiclib_direct(lat1, lon1, azi1, s12):
    result = Geodesic.WGS84.Direct(lat1, lon1, azi1, s12)
    return result["lat2"], result["lon2"]

def append_lat_lon_to_csv_file(input_filename, output_filename, lat_lon_data):
    print(f"Writing CSV data to {output_filename}")
    # Read all lines from the original CSV file, and discard line endings
    # Lines are stored in a list where index 0 is the first line in the file etc.
    with open(input_filename) as csv_file:
        original_csv = [line.strip() for line in csv_file.readlines()]
    # Get the original headings, and add the new ones
    new_headings = "".join([original_csv[0], ",lat,lon\n"])
    # Remove the headings from the list of lines in the original CSV file
    original_csv.pop(0)
    # Check the lines in the CSV file matches the number of rows we have calculated the 
    # new latitude and long for
    assert(len(original_csv) == len(lat_lon_data))
    # Append the calculated lat / lon values to each line in the original CSV data.
    # Make a single string where there is a new line char at the end after every row
    new_csv_string = "\n".join(
        [f"{original_csv},{lat_lon[0]:.8f},{lat_lon[1]:.8f}" \
         for original_csv, lat_lon in zip(original_csv, lat_lon_data)])
    # Write the headings and new data to the new CSV file
    with open(output_filename, "w") as csv_file:
        csv_file.write(new_headings)
        csv_file.write(new_csv_string)

if __name__ == "__main__":

    parser = argparse.ArgumentParser(description='Convert from cartesian to lat / lon')
    parser.add_argument('input_filename', type=str, help='CSV Input file name')
    parser.add_argument('-o', '--output-filename', type=str, help='CSV Output file name (optional)')
    parser.add_argument('latitude', type=float, help='Latitude (Decimal Degrees)')
    parser.add_argument('longitude', type=float, help='Longitude (Decimal Degrees)')
    parser.add_argument('-m', '--method', choices=['reference', 'estimate', 'haversine'], 
                        default="reference", help='Specify a method (reference, estimate, haversine)')
    parser.add_argument('-e', '--error', action='store_true', help='Set this flag to display the" \
                        "difference from the reference implementation')

    args = parser.parse_args()
    output_filename = args.output_filename if args.output_filename else args.input_filename

    # Read X and Y offsets from file
    X, Y = get_x_y_data(args.input_filename)
    # Calculate new latitude and longitude at the X/Y offsets specified
    if args.method == "estimate":
        lat_lon_calc = get_new_lat_long_fast_estimate(args.latitude, args.longitude, X, Y)
    elif args.method == "haversine":
        lat_lon_calc = get_new_lat_long_haversine(args.latitude, args.longitude, X, Y)
    else:
        from geographiclib.geodesic import Geodesic
        lat_lon_calc = get_lat_lon_geographiclib(args.latitude, args.longitude, X, Y)
    # Write data to a new CSV file, with lat and lon columns added to the original data
    append_lat_lon_to_csv_file(args.input_filename, output_filename, lat_lon_calc)

    if args.error:
        print("Calculating error from reference implementation")
        from geographiclib.geodesic import Geodesic
        lat_lon_ref = get_lat_lon_geographiclib(args.latitude, args.longitude, X, Y)
        error = np.array([Geodesic.WGS84.Inverse(r[0], r[1], e[0], e[1])["s12"]
                          for r, e in zip(lat_lon_ref, lat_lon_calc)])
        distance = np.array([Geodesic.WGS84.Inverse(args.latitude, args.longitude, r[0], r[1])["s12"]
                            for r in lat_lon_ref])
        print(f"\tMean error: {np.mean(error):.2f}")
        print(f"\tMedian error: {np.median(error):.2f}")
        print(f"\tMin error: {np.min(error):.2f}")
        print(f"\tMax error: {np.max(error):.2f}")
        
        try:
            import matplotlib.pyplot as plt
            plt.plot(distance, error, "*")
            plt.show()
        except ImportError:
            print("WARNING: Can not plot error vs distance. Matplotlib is not installed")

reference_point.py

#!/usr/bin/env python3

# Tested with python3.11, geographiclib==2.0, matplotlib==3.8.2, numpy==1.26.2
import math
import numpy as np
import csv
import argparse
from geographiclib.geodesic import Geodesic
import matplotlib.pyplot as plt

def get_data(filename):
    # Read the first line of the CSV file
    print(f"Reading CSV data from {filename}")
    try:
        with open(filename) as csv_file:
            reader = csv.reader(csv_file)
            csv_headings = next(reader)
    except:
        print(f"ERROR: Could not parse {filename}")
        print("Check file exists and is a valid CSV file")
        exit()
    valid_x_col_names = ["x", "x-position", "x-offset", "x-positions", "x-offsets", "x position", 
                         "x offset", "x positions", "x offsets", "offset x", "position x"]
    valid_y_col_names = ["y", "y-position", "y-offset", "y-position", "y-offset", "y position", 
                         "y offset", "y positions", "y offsets", "offset y", "position y"]
    valid_lat_col_names = ["lat", "latitude", "lat1"]
    valid_lon_col_names = ["lon", "lng", "long", "longitude", "lon1", "lng1"]

    heading_names = ["X-Offset", "Y-Offset", "Latitude", "Longitude"]
    valid_headings = [valid_x_col_names, valid_y_col_names, valid_lat_col_names, valid_lon_col_names]
    heading_indices = [None, None, None, None]

    for index, csv_heading in enumerate(csv_headings):
        for heading_index, valid_heading in enumerate(valid_headings):
            if csv_heading.lower() in valid_heading:
                heading_indices[heading_index] = (index, csv_heading)

    for heading_name, heading_index, valid_heading in zip(heading_names, heading_indices, valid_headings):
        if heading_index is None:
            print(f"ERROR: Can't find {heading_name} Position heading. Valid headings are: ", end="")
            for name in valid_heading:
                print(f"'{name}' ", end="")
            print()
            exit()
        else:
            print(f"Using {heading_name} heading: '{heading_index[1]}'")
            
    x = np.loadtxt(filename, delimiter=',', skiprows=1, usecols=heading_indices[0][0])
    y = np.loadtxt(filename, delimiter=',', skiprows=1, usecols=heading_indices[1][0])
    lat = np.loadtxt(filename, delimiter=',', skiprows=1, usecols=heading_indices[2][0])
    lon = np.loadtxt(filename, delimiter=',', skiprows=1, usecols=heading_indices[3][0])
    if len(x) != len(y) or len(x) != len(lat) or len(x) != len(lon):
        print("ERROR: Failed to parse CSV file")
        exit()
    return x, y, lat, lon

def get_reference_point(lat2, lon2, X, Y):
    # https://geographiclib.sourceforge.io/C++/doc/
    # Transform to latitude and longitude
    # X and Y are the offset from the reference, so negate them to get bearing 
    # towards reference point
    bearing = np.degrees(np.arctan2(-X,-Y))
    distance = np.sqrt(np.power(X, 2)+np.power(Y,2))
    lat_lon_data = np.array(
        [_get_geographiclib_direct(lat, lon, azi1, s12) 
         for lat, lon, azi1, s12 in zip(lat2, lon2, bearing, distance)])
    return lat_lon_data

def _get_geographiclib_direct(lat1, lon1, azi1, s12):
    result = Geodesic.WGS84.Direct(lat1, lon1, azi1, s12)
    return result["lat2"], result["lon2"]

if __name__ == "__main__":

    parser = argparse.ArgumentParser(description='Find Lat / Lon of reference point')
    parser.add_argument('input_filename', type=str, help='CSV Input file name')
    parser.add_argument('-p',  '--percentage', type=float, default=10.0, 
                        help='Only use X percent of the closest points to calculate the reference')
    args = parser.parse_args()

    # Read X and Y offsets from file
    x, y, lat, lon = get_data(args.input_filename)

    # Sort data by the distance from the reference point
    mag = np.sqrt(np.power(x,2),np.power(y,2))
    data = np.column_stack((mag,x,y,lat,lon))
    data = data[data[:,0].argsort()]

    # Calculate the reference point from all points
    all_reference_points = get_reference_point(X=data[:,1], Y=data[:,2], lat2=data[:,3], lon2=data[:,4])
    all_reference_latitudes = all_reference_points[:, 0]
    all_reference_longitudes = all_reference_points[:, 1]

    # Calculate the reference point from the closest 10% of reference points
    n = int(len(data) // (100 / args.percentage))
    close_reference_points = get_reference_point(X=data[:n,1], Y=data[:n,2], lat2=data[:n,3], lon2=data[:n,4])
    close_reference_latitudes = close_reference_points[:, 0]
    close_reference_longitudes = close_reference_points[:, 1]

    # Get the average reference point
    ref_lat = np.mean(close_reference_latitudes)
    ref_lon = np.mean(close_reference_longitudes)
    print(f"Reference point: {ref_lat}, {ref_lon}")

    # Plot the results
    plt.plot(all_reference_longitudes, all_reference_latitudes, "b*", label="All")
    plt.plot(close_reference_longitudes, close_reference_latitudes, "g*", label="Close")
    plt.plot(np.mean(ref_lon), np.mean(ref_lat), "r*", label="Average")
    plt.legend()
    plt.show()