project_path.py

from camera import img_from_device, denormalize, view_frame_from_device_frame
import orientation as orient
import numpy as np
import cv2

example_segment = "/mnt/Bulk Storage/commaai/comma2k19/Chunk_1/b0c9d2329ad1606b_2018-07-27--06-50-48/9/"

frame_times = np.load(example_segment + 'global_pose/frame_times')
frame_positions = np.load(example_segment + 'global_pose/frame_positions')
frame_orientations = np.load(example_segment + 'global_pose/frame_orientations')

radar_returns = np.load(example_segment + '/processed_log/CAN/radar/value')
radar_time_stamps = np.load(example_segment + '/processed_log/CAN/radar/t')

print(radar_returns[:10])
print(radar_time_stamps[:10])


ecef_from_local = orient.rot_from_quat(frame_orientations[0])
local_from_ecef = ecef_from_local.T
frame_positions_local = np.einsum('ij,kj->ki', local_from_ecef, frame_positions - frame_positions[0])

def draw_path(device_path, img, width=1, height=1.2, fill_color=(128,0,255), line_color=(0,255,0)):
    device_path_l = device_path + np.array([0, 0, height])                                                                    
    device_path_r = device_path + np.array([0, 0, height])                                                                    
    device_path_l[:,1] -= width                                                                                               
    device_path_r[:,1] += width

    img_points_norm_l = img_from_device(device_path_l)
    img_points_norm_r = img_from_device(device_path_r)
    img_pts_l = denormalize(img_points_norm_l)
    img_pts_r = denormalize(img_points_norm_r)

    # filter out things rejected along the way
    valid = np.logical_and(np.isfinite(img_pts_l).all(axis=1), np.isfinite(img_pts_r).all(axis=1))
    img_pts_l = img_pts_l[valid].astype(int)
    img_pts_r = img_pts_r[valid].astype(int)

    for i in range(1, len(img_pts_l)):
        u1,v1,u2,v2 = np.append(img_pts_l[i-1], img_pts_r[i-1])
        u3,v3,u4,v4 = np.append(img_pts_l[i], img_pts_r[i])
        pts = np.array([[u1,v1],[u2,v2],[u4,v4],[u3,v3]], np.int32).reshape((-1,1,2))
        cv2.fillPoly(img,[pts],fill_color)
        cv2.polylines(img,[pts],True,line_color)

img = cv2.imread(example_segment + 'preview.png')
draw_path(frame_positions_local[11:250], img)
cv2.imshow('image',img)
cv2.waitKey(27)