<commit_before>import glob
#import preprocess as pp
#import preprocess_many_labels as pml
import numpy as np
import os
import os.path
import pickle
import random
import shutil
import sys
import tempfile
from scipy.io import loadmat
from scipy.misc import toimage, imresize
from CameraParams import getCameraParams
from CameraReprojection import *
from GPSReader import *
from GPSReprojection import *
from GPSTransforms import *
from RandomVideoReader import *
from WGS84toENU import *
from WarpUtils import *
from WarpedVideoReader import *
from cv2 import imshow, waitKey, resize
import pdb
from copy import deepcopy

def outputDistances(distances, framenum, meters_per_point, points_fwd, start_offset):
    output = []
    point_num = 1
    dist = 0

    framenum += 1
    while framenum < distances.size and point_num <= points_fwd:
        dist += distances[framenum]
        if point_num * meters_per_point <= dist - start_offset:
            output.append(framenum)
            point_num += 1
        else:
            framenum += 1
        
    return output

def runBatch(video_reader, gps_dat, cam, output_base, start_frame, final_frame, left_lanes, right_lanes, ldist, rdist, tr):
    meters_per_point = 10
    points_fwd = 8
    frames_per_second = 50
    distances = GPSVelocities(gps_dat) / frames_per_second
    pts = GPSPos(gps_dat, cam, gps_dat[0, :]) # points wrt camera
    count = 0
    output_num = 0
    imgs = []
    labels = []
    success = True
    while True:
        #(success, I, frame, P) = video_reader.getNextFrame()
        (success, I) = video_reader.getNextFrame()
        frame = 10*count
        #frame = count
        P = np.eye(3)
        if count % 160 == 0:
            print count
        if success == False or frame >= tr.shape[0]:
            print success, frame, tr.shape[0]
            break
        if frame < start_frame or (final_frame != -1 and frame >= final_frame):
            continue

        important_frames = (outputDistances(distances, frame, meters_per_point, points_fwd, 2))
        if len(important_frames) < points_fwd:
            continue

        important_left = []
        important_right = []

        velocities = gps_dat[important_frames,4:7]
        velocities[:,[0, 1]] = velocities[:,[1, 0]]
        sideways = np.cross(velocities, np.array([0,0,1]), axisa=1)
        sideways/= np.sqrt((sideways ** 2).sum(-1))[..., np.newaxis]
        sideways = GPSPosCamera(ENU2IMU(np.transpose(sideways), gps_dat[0, :]), cam) # sideways vector wrt camera
        center = GPSPos(gps_dat[important_frames,:], cam, gps_dat[0, :]) # points wrt imu
        for ind in xrange(len(important_frames)):
            fr = important_frames[ind]
            min_val = max(fr - 50, 0)
            max_val = min(fr + 50, left_lanes.shape[0] - 1)

            l_distances = np.cross(left_lanes[min_val:max_val, 0:3] - np.transpose(center[:,ind]), np.transpose(sideways[:, ind]), axisa=1)
            l_distances = np.sqrt((l_distances ** 2).sum(-1))
            r_distances = np.cross(right_lanes[min_val:max_val, 0:3] - np.transpose(center[:,ind]), np.transpose(sideways[:, ind]), axisa=1)
            r_distances = np.sqrt((r_distances ** 2).sum(-1))
            important_left.append(np.argmin(l_distances)+min_val)
            important_right.append(np.argmin(r_distances)+min_val)
        important_left = np.array(important_left)
        important_right = np.array(important_right)
        max_idx = max(max(np.max(important_frames), np.max(important_left)), np.max(important_right))
        if max_idx >= left_lanes.shape[0] or max_idx >= right_lanes.shape[0]:
            print 'maxing out'
            continue
        temp_left = np.linalg.solve(tr[frame, :, :], left_lanes[important_left, :].transpose())
        temp_right = np.linalg.solve(tr[frame, :, :], right_lanes[important_right, :].transpose())

        gps_vals = warpPoints(P, GPSColumns(gps_dat[important_frames], cam, gps_dat[frame, :])[0:2])
        left_vals = warpPoints(P, PointsMask(temp_left[0:3, :], cam)[0:2])
        right_vals = warpPoints(P, PointsMask(temp_right[0:3, :], cam)[0:2])

        gps_vals = (gps_vals / 4).astype(np.int32)
        left_vals = (left_vals / 4).astype(np.int32)
        right_vals = (right_vals / 4).astype(np.int32)
        left_vals = left_vals.clip(0,500)
        right_vals = right_vals.clip(0,500)
        gps_vals = gps_vals.clip(0,500)
        gps_vals[0, gps_vals[0, :] >= 320] = 319
        gps_vals[1, gps_vals[1, :] >= 240] = 239
        left_vals[0, left_vals[0, :] >= 320] = 319
        left_vals[1, left_vals[1, :] >= 240] = 239<commit_msg>Fixed bug in computing the sideways vector.<commit_after>import glob
#import preprocess as pp
#import preprocess_many_labels as pml
import numpy as np
import os
import os.path
import pickle
import random
import shutil
import sys
import tempfile
from scipy.io import loadmat
from scipy.misc import toimage, imresize
from CameraParams import getCameraParams
from CameraReprojection import *
from GPSReader import *
from GPSReprojection import *
from GPSTransforms import *
from RandomVideoReader import *
from WGS84toENU import *
from WarpUtils import *
from WarpedVideoReader import *
from cv2 import imshow, waitKey, resize
import pdb
from copy import deepcopy

def outputDistances(distances, framenum, meters_per_point, points_fwd, start_offset):
    output = []
    point_num = 1
    dist = 0

    framenum += 1
    while framenum < distances.size and point_num <= points_fwd:
        dist += distances[framenum]
        if point_num * meters_per_point <= dist - start_offset:
            output.append(framenum)
            point_num += 1
        else:
            framenum += 1
        
    return output

def runBatch(video_reader, gps_dat, cam, output_base, start_frame, final_frame, left_lanes, right_lanes, ldist, rdist, tr):
    meters_per_point = 10
    points_fwd = 8
    frames_per_second = 50
    distances = GPSVelocities(gps_dat) / frames_per_second
    pts = GPSPos(gps_dat, cam, gps_dat[0, :]) # points wrt camera
    count = 0
    output_num = 0
    imgs = []
    labels = []
    success = True
    while True:
        #(success, I, frame, P) = video_reader.getNextFrame()
        (success, I) = video_reader.getNextFrame()
        frame = 10*count
        #frame = count
        P = np.eye(3)
        if count % 160 == 0:
            print count
        if success == False or frame >= tr.shape[0]:
            print success, frame, tr.shape[0]
            break
        if frame < start_frame or (final_frame != -1 and frame >= final_frame):
            continue

        important_frames = (outputDistances(distances, frame, meters_per_point, points_fwd, 2))
        if len(important_frames) < points_fwd:
            continue

        important_left = []
        important_right = []

        velocities = gps_dat[important_frames,4:7]
        velocities[:,[0, 1]] = velocities[:,[1, 0]]
        sideways = np.cross(velocities, np.array([0,0,1]), axisa=1)
        sideways/= np.sqrt((sideways ** 2).sum(-1))[..., np.newaxis]
        vel_start = ENU2IMU(np.transpose(velocities), gps_dat[0,:])
        vel_current = ENU2IMU(np.transpose(velocities), gps_dat[frame,:])
        sideways_start = GPSPosCamera(np.cross(vel_start, np.array([0,0,1]), axisa=0).transpose(), cam) # sideways vector wrt starting frame (camera)
        sideways_current = GPSPosCamera(np.cross(vel_current, np.array([0,0,1]), axisa=0).transpose(), cam) # sideways vector wrt starting frame (camera)
        sideways_start /= np.sqrt((sideways_start ** 2).sum(0))[np.newaxis, ...]
        sideways_current /= np.sqrt((sideways_current ** 2).sum(0))[np.newaxis, ...] # save sideways_current
        center = GPSPos(gps_dat[important_frames,:], cam, gps_dat[0, :]) # points wrt imu
        for ind in xrange(len(important_frames)):
            fr = important_frames[ind]
            min_val = max(fr - 50, 0)
            max_val = min(fr + 50, left_lanes.shape[0] - 1)

            l_distances = np.cross(left_lanes[min_val:max_val, 0:3] - np.transpose(center[:,ind]), np.transpose(sideways_start[:, ind]), axisa=1)
            l_distances = np.sqrt((l_distances ** 2).sum(-1))
            r_distances = np.cross(right_lanes[min_val:max_val, 0:3] - np.transpose(center[:,ind]), np.transpose(sideways_start[:, ind]), axisa=1)
            r_distances = np.sqrt((r_distances ** 2).sum(-1))
            important_left.append(np.argmin(l_distances)+min_val)
            important_right.append(np.argmin(r_distances)+min_val)
        important_left = np.array(important_left)
        important_right = np.array(important_right)
        max_idx = max(max(np.max(important_frames), np.max(important_left)), np.max(important_right))
        if max_idx >= left_lanes.shape[0] or max_idx >= right_lanes.shape[0]:
            print 'maxing out'
            continue

        temp_left = np.linalg.solve(tr[frame, :, :], left_lanes[important_left, :].transpose()) # save temp_left [0:3,:]
        #temp_left[0:3, :] += sideways_current*0.8

        temp_gps = GPSPos(gps_dat[important_frames], cam, gps_dat[frame, :]) # save temp_gps [0:3,:]

        temp_right = np.linalg.solve(tr[frame, :, :], right_lanes[important_right, :].transpose()) # save temp_right [0:3,:]
        #temp_right[0:3, :] -= sideways_current*0.8
        
        gps_vals = warpPoints(P, PointsMask(temp_gps[0:3, :], cam)[0:2]) # save P
        left_vals = warpPoints(P, PointsMask(temp_left[0:3, :], cam)[0:2])
        right_vals = warpPoints(P, PointsMask(temp_right[0:3, :], cam)[0:2])

        gps_vals = (gps_vals / 4).astype(np.int32)
        left_vals = (left_vals / 4).astype(np.int32)
        right_vals = (right_vals / 4).astype(np.int32)
        left_vals = left_vals.clip(0,500)
        right_vals = right_vals.clip(0,500)
        gps_vals = gps_vals.clip(0,500)
        gps_vals[0, gps_vals[0, :] >= 320] = 319
        gps_vals[1, gps_vals[1, :] >= 240] = 239
        left_vals[0, left_vals[0, :] >= 320] = 319
        left_vals[1, left_vals[1, :] >= 240] = 239
{'fraction_non_alphanumeric': 0.1261826511303655, 'fraction_numerical': 0.027586893735683696, 'mean_word_length': 3.0119856172592887, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 19, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2336181', 'n_tokens_mistral': 3597, 'n_tokens_neox': 3417, 'n_words': 728}