<commit_before>from Q50_config import *
import sys, os
from GPSReader import *
from GPSReprojection import *
from GPSTransforms import *
from VideoReader import *
from RadarTransforms import *
from LidarTransforms import *
from ColorMap import *
from transformations import euler_matrix
import numpy as np
import cv2
from ArgParser import *

(Rx,Ry,Rz) = (0, 0, -.015)

def cloudToPixels(cam, pts_wrt_cam): 

    width = 4
    (pix, J)  = cv2.projectPoints(pts_wrt_cam.transpose(), np.array([0.0,0.0,0.0]), np.array([0.0,0.0,0.0]), cam['KK'], cam['distort'])

    pix = pix.transpose()
    pix = np.around(pix[:, 0, :])
    pix = pix.astype(np.int32)
    mask = np.logical_and(True, pix[0,:] > 0 + width/2)
    mask = np.logical_and(mask, pix[1,:] > 0 + width/2)
    mask = np.logical_and(mask, pix[0,:] < 1279 - width/2)
    mask = np.logical_and(mask, pix[1,:] < 959 - width/2)
    mask = np.logical_and(mask, pts_wrt_cam[2,:] > 0)
    dist_sqr = np.sum( pts_wrt_cam[0:3, :] ** 2, axis = 0)
    mask = np.logical_and(mask, dist_sqr > 3)

    return (pix, mask)

def transformLidarPointsToCameraPoints(points, cam):
    pts_wrt_camera_t = points + cam['displacement_from_l_to_c_in_lidar_frame']
    pts_wrt_camera_t = dot(R_to_c_from_l(cam), pts_wrt_camera_t.transpose())

    return pts_wrt_camera_t

if __name__ == '__main__':
    args = parse_args(sys.argv[1], sys.argv[2])
    cam_num = int(sys.argv[2][-5])

    cam = GetQ50CameraParams()[cam_num - 1] 
    video_reader = VideoReader(args['video'])
    rdr_map = loadRDRCamMap(args['map'])
    
    writer = cv2.VideoWriter('radar_test.avi', cv.CV_FOURCC('X','V', 'I', 'D'),
                    50.0, (1280,960) )

    while True:
        for t in xrange(5):
            (success, I) = video_reader.getNextFrame()

        all_pix = None

        frame_num = video_reader.framenum

        radar_data = loadRDR(rdr_map[frame_num])[0]

        if radar_data.shape[0] > 0:
            radar_data[:, :3] = calibrateRadarPts(radar_data[:, :3], (Rx, Ry, Rz))

            # print radar_data[:, 0:3]
            back_pts = transformLidarPointsToCameraPoints(radar_data[:,0:3], cam)
            
            front_right_pts = np.array(radar_data[:,0:3])
            front_right_pts[:,0] += radar_data[:,3]
            front_right_pts[:,1] += radar_data[:,4] / 2.
            front_right_pts = transformLidarPointsToCameraPoints(front_right_pts, cam)
            
            front_left_pts = np.array(radar_data[:,0:3])
            front_left_pts[:,0] += radar_data[:,3]
            front_left_pts[:,1] -= radar_data[:,4] / 2.
            front_left_pts = transformLidarPointsToCameraPoints(front_left_pts, cam)

            right_pts = np.array(radar_data[:,0:3])
            right_pts[:,1] += radar_data[:,4] / 2.<commit_msg>Updated ProjectRadarOnVideo to use the new config file<commit_after>from Q50_config import *
import sys, os
from GPSReader import *
from GPSReprojection import *
from GPSTransforms import *
from VideoReader import *
from RadarTransforms import *
from LidarTransforms import *
from ColorMap import *
from transformations import euler_matrix
import numpy as np
import cv2
from ArgParser import *

(Rx,Ry,Rz) = (0, 0, -.015)

def cloudToPixels(cam, pts_wrt_cam): 

    width = 4
    (pix, J)  = cv2.projectPoints(pts_wrt_cam.transpose(), np.array([0.0,0.0,0.0]), np.array([0.0,0.0,0.0]), cam['KK'], cam['distort'])

    pix = pix.transpose()
    pix = np.around(pix[:, 0, :])
    pix = pix.astype(np.int32)
    mask = np.logical_and(True, pix[0,:] > 0 + width/2)
    mask = np.logical_and(mask, pix[1,:] > 0 + width/2)
    mask = np.logical_and(mask, pix[0,:] < 1279 - width/2)
    mask = np.logical_and(mask, pix[1,:] < 959 - width/2)
    mask = np.logical_and(mask, pts_wrt_cam[2,:] > 0)
    dist_sqr = np.sum( pts_wrt_cam[0:3, :] ** 2, axis = 0)
    mask = np.logical_and(mask, dist_sqr > 3)

    return (pix, mask)

def transformLidarPointsToCameraPoints(points, cam):
    pts_wrt_camera_t = points + cam['displacement_from_l_to_c_in_lidar_frame']
    pts_wrt_camera_t = dot(R_to_c_from_l(cam), pts_wrt_camera_t.transpose())

    return pts_wrt_camera_t

if __name__ == '__main__':
    args = parse_args(sys.argv[1], sys.argv[2])
    cam_num = int(sys.argv[2][-5])

    params = args['params']
    cam = params['cam'][cam_num - 1]

    video_reader = VideoReader(args['video'])
    rdr_map = loadRDRCamMap(args['map'])
    
    writer = cv2.VideoWriter('radar_test.avi', cv.CV_FOURCC('X','V', 'I', 'D'),
                    50.0, (1280,960) )

    while True:
        for t in xrange(5):
            (success, I) = video_reader.getNextFrame()

        all_pix = None

        frame_num = video_reader.framenum

        radar_data = loadRDR(rdr_map[frame_num])[0]

        if radar_data.shape[0] > 0:
            radar_data[:, :3] = calibrateRadarPts(radar_data[:, :3], params['radar'])

            # print radar_data[:, 0:3]
            back_pts = transformLidarPointsToCameraPoints(radar_data[:,0:3], cam)
            
            front_right_pts = np.array(radar_data[:,0:3])
            front_right_pts[:,0] += radar_data[:,3]
            front_right_pts[:,1] += radar_data[:,4] / 2.
            front_right_pts = transformLidarPointsToCameraPoints(front_right_pts, cam)
            
            front_left_pts = np.array(radar_data[:,0:3])
            front_left_pts[:,0] += radar_data[:,3]
            front_left_pts[:,1] -= radar_data[:,4] / 2.
            front_left_pts = transformLidarPointsToCameraPoints(front_left_pts, cam)

            right_pts = np.array(radar_data[:,0:3])
            right_pts[:,1] += radar_data[:,4] / 2.
{'fraction_non_alphanumeric': 0.1475876802563646, 'fraction_numerical': 0.03382588570411252, 'mean_word_length': 3.097738876732312, 'pattern_counts': {'":': 0, '<': 7, '<?xml version=': 0, '>': 13, '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': '642847', 'n_tokens_mistral': 2221, 'n_tokens_neox': 2132, 'n_words': 384}