waypoints_near_track.py

#!/usr/bin/env python
import numpy as np
import pandas as pd
import gpxpy

import argparse

R = 6365E3
RADIUS = 1000


def main():
    parser = argparse.ArgumentParser()

    parser.add_argument('--radius', '-r', default=RADIUS, type=int)
    parser.add_argument('waypoint_gpx')
    parser.add_argument('track_gpx')
    parser.add_argument('output')

    args = parser.parse_args()

    waypoints = load_gpx_waypoints(args.waypoint_gpx)
    track_points = load_gpx_track(args.track_gpx)

    distances = distances_between_coord(
            waypoints['Latitude'], waypoints['Longitude'],
            track_points['Latitude'], track_points['Longitude']
            )

    waypoints['Distance to Route'] = np.min(distances, axis=1)

    nearby = waypoints[waypoints['Distance to Route'] < args.radius]
    save_waypoint_gpx(nearby, args.output)


def load_gpx_waypoints(gpx_file) -> pd.DataFrame:
    with open(gpx_file, 'r') as f:
        gpx = gpxpy.parse(f)

        waypoints = [{
            "Name": waypoint.name,
            "Latitude": waypoint.latitude,
            "Longitude": waypoint.longitude,
            "Elevation": waypoint.elevation,
            "Comment": waypoint.comment
            } for waypoint in gpx.waypoints]

        return pd.DataFrame(waypoints)


def load_gpx_track(gpx_file) -> pd.DataFrame:
    with open(gpx_file, 'r') as f:
        gpx = gpxpy.parse(f)

        points = [{
            "Name": point.name,
            "Latitude": point.latitude,
            "Longitude": point.longitude,
            "Elevation": point.elevation,
            "Comment": point.comment
            } for point in gpx.tracks[0].segments[0].points]

        return pd.DataFrame(points)


def save_waypoint_gpx(dataframe, filename):
    gpx = gpxpy.gpx.GPX()

    def create_waypoint(params):
        return gpxpy.gpx.GPXWaypoint(
            params[1],
            params[2],
            name=params[0],
            comment=params[4]
        )

    gpx.waypoints = list(dataframe.apply(create_waypoint, axis=1))

    with open(filename, 'w') as f:
        f.write(gpx.to_xml())


def distances_between_coord(lats1, longs1, lats2, longs2):
    """Calculate distances between all combinations of the two lat-long combinations."""
    assert len(lats1) == len(longs1)
    assert len(lats2) == len(longs2)
    lats1 = np.radians(np.array(lats1))
    longs1 = np.radians(np.array(longs1))
    lats2 = np.radians(np.array(lats2))
    longs2 = np.radians(np.array(longs2))

    len1 = len(lats1)
    len2 = len(lats2)

    cos_lats1 = np.cos(lats1)
    cos_lats2 = np.cos(lats2)

    dlat = np.empty((len1, len2))
    dlong = np.empty((len1, len2))

    tiled_lats1 = np.tile(np.atleast_2d(lats1).T, (1, len2,))
    tiled_lats2 = np.tile(np.atleast_2d(lats2), (len1, 1))

    tiled_longs1 = np.tile(np.atleast_2d(longs1).T, (1, len2))
    tiled_longs2 = np.tile(np.atleast_2d(longs2), (len1, 1))

    dlat = tiled_lats2 - tiled_lats1
    del tiled_lats1, tiled_lats2

    dlong = tiled_longs2 - tiled_longs1
    del tiled_longs1, tiled_longs2

    a = np.sin(dlat/2)**2 + np.outer(cos_lats1, cos_lats2) * np.sin(dlong/2)**2
    return 2 * R * np.arctan2(np.sqrt(a), np.sqrt(1-a))


if __name__ == "__main__":
    main()