Merges the map-matched trajectory to the corresponding map nodes

merge_trajectory.py

def merge_trajectory(trajectory: Trajectory,
                     map_lat: np.ndarray,
                     map_lon: np.ndarray) -> list[list[LatLon]]:
    segments: list[list[LatLon]] = []
    j = 0
    for i in range(trajectory.lat.shape[0]-1):
        pt0 = LatLon(float(trajectory.lat[i]), float(trajectory.lon[i]))
        pt1 = LatLon(float(trajectory.lat[i+1]), float(trajectory.lon[i+1]))
        seg_len = pt0.haversine(*pt1.to_tuple())

        segment = [pt0]
        while j < map_lat.shape[0]:
            node = LatLon(float(map_lat[j]), float(map_lon[j]))
            len_ini = pt0.haversine(*node.to_tuple())
            len_end = pt1.haversine(*node.to_tuple())

            if len_ini <= seg_len and len_end <= seg_len:
                segment.append(node)
                j += 1
            else:
                segment.append(pt1)
                segments.append(segment)
                break
        else:
            segment.append(pt1)
            segments.append(segment)
    return segments