<commit_before>#usage
# python LidarReprojectCalibrate.py <dir-to-data> <basename> <start frame> 

from Q50_config import *
import sys, os
from GPSReader import *
from GPSTransforms import *
from VideoReader import *
from LidarTransforms import *
from ColorMap import *
from transformations import euler_matrix
import numpy as np
import cv2
import socket
import time
from ArgParser import *
import SocketServer
import threading
import random

global rx, ry, rz, crx, crz, R, cR, paramInit, port

paramInit = False
port = 3000 + int(random.random()*10000)

def ParametersToString(rx,ry,rz,crx,cry,crz):
    return "%f,%f,%f,%f,%f,%f\n" % (rx,ry,rz,crx,cry,crz)

class RequestHandler(SocketServer.BaseRequestHandler):
    def handle(self):
        global rx, ry, rz, crx, cry, crz, R, cR, paramInit 

        data = self.request[0].strip()
        print data
        (rx,ry,rz,crx,cry,crz) = map(lambda x: float(x), data.split(','))
        
        R = euler_matrix(rx,ry,rz)[0:3,0:3].transpose()
        #cR = euler_matrix(crx, cry, crz)[0:3,0:3]
        cR = euler_matrix(crx, cry, crz)[0:3,0:3]
        paramInit = True


class ThreadedServer(threading.Thread):
    def __init__(self, port):
        self.server = None
        self.port = port
        threading.Thread.__init__(self)
    def run(self):
        if self.server == None:
            address = ('localhost', self.port)
            self.server = SocketServer.UDPServer(address, RequestHandler)
        print 'starting server'
        self.server.serve_forever()


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, pix[1,:] < 1039 - width/2)
    #mask = np.logical_and(mask, pix[0,:] < 2079 - 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 lidarPtsToPixels(pts_wrt_imu_0, imu_transforms_t, T_from_i_to_l, cam):
    # Transform points back to imu_t
    pts_wrt_imu_t = np.dot(np.linalg.inv(imu_transforms_t), pts_wrt_imu_0)
    #pts_wrt_imu_t = pts_wrt_imu_0

    # transform points from imu_t to lidar_t
    pts_wrt_lidar_t = np.dot(T_from_i_to_l, pts_wrt_imu_t)

    # transform points from lidar_t to camera_t
    pts_wrt_camera_t = pts_wrt_lidar_t.transpose()[:, 0:3] + cam['displacement_from_l_to_c_in_lidar_frame']
    pts_wrt_camera_t = np.dot(cR, np.dot(R_to_c_from_l_old(0), 
            pts_wrt_camera_t.transpose()))

    pts_wrt_camera_t = np.vstack((pts_wrt_camera_t,
        np.ones((1, pts_wrt_camera_t.shape[1]))))
    pts_wrt_camera_t = dot(cam['E'], pts_wrt_camera_t)
    pts_wrt_camera_t = pts_wrt_camera_t[0:3,:]

    # reproject camera_t points in camera frame
    (pix, mask) = cloudToPixels(cam, pts_wrt_camera_t)

    return (pix, mask)

def lidarPtsToPixels_old(pts_wrt_lidar_t, imu_transforms_t, cam):
    # transform points from lidar_t to camera_t
    pts_wrt_camera_t = pts_wrt_lidar_t.transpose()[:, 0:3] + cam['displacement_from_l_to_c_in_lidar_frame']
    pts_wrt_camera_t = np.dot(cR, np.dot(R_to_c_from_l_old(0), 
            pts_wrt_camera_t.transpose()))

    pts_wrt_camera_t = np.vstack((pts_wrt_camera_t,
        np.ones((1,pts_wrt_camera_t.shape[1]))))
    pts_wrt_camera_t = dot(cam['E'], pts_wrt_camera_t)
    pts_wrt_camera_t = pts_wrt_camera_t[0:3,:]

    # reproject camera_t points in camera frame
    (pix, mask) = cloudToPixels(cam, pts_wrt_camera_t)

    return (pix, mask)

if __name__ == '__main__':
    args = parse_args(sys.argv[1], sys.argv[2])
    cam_num = int(sys.argv[2][-5])
    video_file = args['video']
    video_reader = VideoReader(video_file)
    params = args['params'] 
    cam = params['cam'][cam_num-1]
    gps_reader = GPSReader(args['gps_mark2'])
    gps_data = gps_reader.getNumericData()
    lidar_loader = LDRLoader(args['frames'])
    imu_transforms = IMUTransforms(gps_data)
    gps_times = utc_from_gps_log_all(gps_data)
    
    # parameter server
    thr = ThreadedServer(port)
    thr.setDaemon(True)
    thr.start()
    time.sleep(1)
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.sendto('PARAMETER_REQUEST:'+str(port)+'\n', ('localhost', 2929))
    while not paramInit:
        time.sleep(1)

    print int(sys.argv[3])
    video_reader.setFrame(int(sys.argv[3]))
    (success, orig) = video_reader.getNextFrame()
    T_from_l_to_i = params['lidar']['T_from_l_to_i']
    T_from_i_to_l = np.linalg.inv(T_from_l_to_i)
    while True:
        I = orig.copy()
        fnum = video_reader.framenum
        t = gps_times[fnum]
        data, data_times = lidar_loader.loadLDRWindow(t-50000, 0.1)

        # Transform data into IMU frame at time t
        pts = data[:, 0:3].transpose()
        pts = np.vstack((pts, np.ones((1, pts.shape[1]))))
        pts = np.dot(T_from_l_to_i, pts)

        # Shift points according to timestamps instead of using transform of full sweep
        #transform_points_by_times(pts, t_data, imu_transforms_mark1, gps_times_mark1)
        transform_points_by_times(pts, data_times, imu_transforms, gps_times)

        (pix, mask) = lidarPtsToPixels(pts, imu_transforms[fnum,:,:], T_from_i_to_l,cam); 
        intensity = data[mask, 3]
        heat_colors = heatColorMapFast(intensity, 0, 100)
        for p in range(4):
            I[pix[1,mask]+p, pix[0,mask], :] = heat_colors[0,:,:]
            I[pix[1,mask], pix[0,mask]+p, :] = heat_colors[0,:,:]
            I[pix[1,mask]+p, pix[0,mask], :] = heat_colors[0,:,:]
            I[pix[1,mask], pix[0,mask]+p, :] = heat_colors[0,:,:]

        cv2.imshow('vid', I)
        key = cv2.waitKey(10)
        if key == -1:
            continue
        key = chr(key & 255)
        if key == 'a':
            cry += 0.001
        elif key == 'd':
            cry -= 0.001
        elif key == 'w':
            crx += 0.001
        elif key == 's':
            crx -= 0.001
        elif key == '+':
            crz += 0.001
        elif key == '_' or key == '-':
            crz -= 0.001
        else:
            continue
    
        print (crx, cry, crz)
        cR = euler_matrix(crx, cry, crz)[0:3,0:3]
        sock.sendto('PARAMETER_UPDATE:'+str(port)+':'+ParametersToString(rx,ry,rz,crx,cry,crz), ('localhost', 2929))
<commit_msg>using lanes to do camera to imu calibration<commit_after>#usage
# python LidarReprojectCalibrate.py <dir-to-data> <basename> <start frame> 

from Q50_config import *
import sys, os
from GPSReader import *
from GPSTransforms import *
from VideoReader import *
from LidarTransforms import *
from ColorMap import *
from transformations import euler_matrix
import numpy as np
import cv2
import socket
import time
from ArgParser import *
import SocketServer
import threading
import random

global rx, ry, rz, crx, crz, R, cR, paramInit, port

paramInit = False
port = 3000 + int(random.random()*10000)

def ParametersToString(rx,ry,rz,crx,cry,crz):
    return "%f,%f,%f,%f,%f,%f\n" % (rx,ry,rz,crx,cry,crz)




def zDistances(self, distances, global_frame, starting_point, meters_per_point, points_fwd):
    output = []
    point_num = 1
    dist = 0
    for pt in xrange(points_fwd):
      dist = pt * meters_per_point+starting_point
      output.append((np.abs(distances-dist)).argmin()+global_frame)


def lanePos(map_pos, imu_transforms_t, cam, T_from_i_to_l):
    # load nearby map frames
    pts_wrt_imu_0 = array(map_pos).transpose()
    pts_wrt_imu_0 = np.vstack((pts_wrt_imu_0, np.ones((1,pts_wrt_imu_0.shape[1]))))
    # transform points from imu_0 to imu_t
    pts_wrt_imu_t = np.dot( np.linalg.inv(imu_transforms_t), pts_wrt_imu_0)
    # transform points from imu_t to lidar_t
    pts_wrt_lidar_t = np.dot(T_from_i_to_l, pts_wrt_imu_t);
    # transform points from lidar_t to camera_t
    pts_wrt_camera_t = pts_wrt_lidar_t.transpose()[:, 0:3] + cam['displacement_from_l_to_c_in_lidar_frame']
    pts_wrt_camera_t = np.dot(cR, np.dot(R_to_c_from_l_old(0),
            pts_wrt_camera_t.transpose()))    
    pts_wrt_camera_t = np.vstack((pts_wrt_camera_t,
        np.ones((1,pts_wrt_camera_t.shape[1]))))
    pts_wrt_camera_t = dot(cam['E'], pts_wrt_camera_t)
    pts_wrt_camera_t = pts_wrt_camera_t[0:3,:]
    return pts_wrt_camera_t






class RequestHandler(SocketServer.BaseRequestHandler):
    def handle(self):
        global rx, ry, rz, crx, cry, crz, R, cR, paramInit 

        data = self.request[0].strip()
        print data
        (rx,ry,rz,crx,cry,crz) = map(lambda x: float(x), data.split(','))
        
        R = euler_matrix(rx,ry,rz)[0:3,0:3].transpose()
        #cR = euler_matrix(crx, cry, crz)[0:3,0:3]
        cR = euler_matrix(crx, cry, crz)[0:3,0:3]
        paramInit = True


class ThreadedServer(threading.Thread):
    def __init__(self, port):
        self.server = None
        self.port = port
        threading.Thread.__init__(self)
    def run(self):
        if self.server == None:
            address = ('localhost', self.port)
            self.server = SocketServer.UDPServer(address, RequestHandler)
        print 'starting server'
        self.server.serve_forever()


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, pix[1,:] < 1039 - width/2)
    #mask = np.logical_and(mask, pix[0,:] < 2079 - 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 lidarPtsToPixels(pts_wrt_imu_0, imu_transforms_t, T_from_i_to_l, cam):
    # Transform points back to imu_t
    pts_wrt_imu_t = np.dot(np.linalg.inv(imu_transforms_t), pts_wrt_imu_0)
    #pts_wrt_imu_t = pts_wrt_imu_0

    # transform points from imu_t to lidar_t
    pts_wrt_lidar_t = np.dot(T_from_i_to_l, pts_wrt_imu_t)

    # transform points from lidar_t to camera_t
    pts_wrt_camera_t = pts_wrt_lidar_t.transpose()[:, 0:3] + cam['displacement_from_l_to_c_in_lidar_frame']
    pts_wrt_camera_t = np.dot(cR, np.dot(R_to_c_from_l_old(0), 
            pts_wrt_camera_t.transpose()))

    pts_wrt_camera_t = np.vstack((pts_wrt_camera_t,
        np.ones((1, pts_wrt_camera_t.shape[1]))))
    pts_wrt_camera_t = dot(cam['E'], pts_wrt_camera_t)
    pts_wrt_camera_t = pts_wrt_camera_t[0:3,:]

    # reproject camera_t points in camera frame
    (pix, mask) = cloudToPixels(cam, pts_wrt_camera_t)

    return (pix, mask)

def lidarPtsToPixels_old(pts_wrt_lidar_t, imu_transforms_t, cam):
    # transform points from lidar_t to camera_t
    pts_wrt_camera_t = pts_wrt_lidar_t.transpose()[:, 0:3] + cam['displacement_from_l_to_c_in_lidar_frame']
    pts_wrt_camera_t = np.dot(cR, np.dot(R_to_c_from_l_old(0), 
            pts_wrt_camera_t.transpose()))

    pts_wrt_camera_t = np.vstack((pts_wrt_camera_t,
        np.ones((1,pts_wrt_camera_t.shape[1]))))
    pts_wrt_camera_t = dot(cam['E'], pts_wrt_camera_t)
    pts_wrt_camera_t = pts_wrt_camera_t[0:3,:]

    # reproject camera_t points in camera frame
    (pix, mask) = cloudToPixels(cam, pts_wrt_camera_t)

    return (pix, mask)

if __name__ == '__main__':
    args = parse_args(sys.argv[1], sys.argv[2])
    cam_num = int(sys.argv[2][-5])
    video_file = args['video']
    video_reader = VideoReader(video_file)
    params = args['params'] 
    cam = params['cam'][cam_num-1]
    #gps_reader = GPSReader(args['gps_mark2'])
    gps_filename= args['gps']
    gps_reader = GPSReader(gps_filename)
    prefix = gps_filename[0:-8]
    gps_data = gps_reader.getNumericData()
    lidar_loader = LDRLoader(args['frames'])
    imu_transforms = IMUTransforms(gps_data)
    gps_times = utc_from_gps_log_all(gps_data)
    
    lane_filename = prefix+'_multilane_points_done.npz'
    lanes = np.load(lane_filename)

    # parameter server
    thr = ThreadedServer(port)
    thr.setDaemon(True)
    thr.start()
    time.sleep(1)
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.sendto('PARAMETER_REQUEST:'+str(port)+'\n', ('localhost', 2929))
    while not paramInit:
        time.sleep(1)

    print int(sys.argv[3])
    video_reader.setFrame(int(sys.argv[3]))
    (success, orig) = video_reader.getNextFrame()
    T_from_l_to_i = params['lidar']['T_from_l_to_i']
    T_from_i_to_l = np.linalg.inv(T_from_l_to_i)
    fwd_range=100
    colors = [(255,0,0),(0,255,0),(0,0,255),(255,255,0),(255,0,255),(0,255,255),(128,128,255),(128,255,128),(255,128,128),(128,128,0),(128,0,128),(0,128,128),(0,128,255),(0,255,128),(128,0,255),(128,255,0),(255,0,128),(255,128,0)]
    while True:
        
      I = orig.copy()
      fnum = video_reader.framenum
      t = gps_times[fnum]

      ids = range(fnum, fnum+300)
      for l in range(lanes['num_lanes']):
        lane_key = 'lane'+str(l)
        lane = lanes[lane_key]
        # find the appropriate portion on the lane (close to the position of car, in front of camera, etc)
        # find the closest point on the lane to the two end-points on the trajectory of car. ideally this should be done before-hand to increase efficiency.
        dist_near = np.sum((lane-imu_transforms[ids[0],0:3,3])**2, axis=1) # find distances of lane to current 'near' position.
        dist_far = np.sum((lane-imu_transforms[ids[-1],0:3,3])**2, axis=1) # find distances of lane to current 'far' position.
        dist_mask = np.where(dist_near<=(fwd_range**2))[0]# only consider points to be valid within fwd_range from the 'near' position
        if len(dist_mask)==0:
          continue
        nearid = np.argmin(dist_near[dist_mask]) # for those valid points, find the one closet to 'near' position.
        farid = np.argmin(dist_far[dist_mask]) #and far position
        lids = range(dist_mask[nearid], dist_mask[farid]+1) # convert back to global id and make it into a consecutive list. 
        lane3d = lanePos(lane[lids,:], imu_transforms[fnum,:,:], cam,T_from_i_to_l) # lane markings in current camera frame
        if np.all(lane3d[2,:]<=0):
          continue
        lane3d = lane3d[:,lane3d[2,:]>0] # make sure in front of camera
        (pix, mask) = cloudToPixels(cam, lane3d)
        for p in range(4):
            I[pix[1,mask]+p, pix[0,mask], :] = colors[(l)%18]
            I[pix[1,mask], pix[0,mask]+p, :] = colors[(l)%18]
            I[pix[1,mask]+p, pix[0,mask], :] = colors[(l)%18]
            I[pix[1,mask], pix[0,mask]+p, :] = colors[(l)%18]

      cv2.imshow('vid', I)
      key = cv2.waitKey(10)
      if key == -1:
        continue
      key = chr(key & 255)
      if key == 'a':
        cry += 0.001
      elif key == 'd':
        cry -= 0.001
      elif key == 'w':
        crx += 0.001
      elif key == 's':
        crx -= 0.001
      elif key == '+':
        crz += 0.001
      elif key == '_' or key == '-':
        crz -= 0.001
      else:
        continue
    
      print (crx, cry, crz)
      cR = euler_matrix(crx, cry, crz)[0:3,0:3]
      sock.sendto('PARAMETER_UPDATE:'+str(port)+':'+ParametersToString(rx,ry,rz,crx,cry,crz), ('localhost', 2929))

{'fraction_non_alphanumeric': 0.14231523044825187, 'fraction_numerical': 0.033065688815831065, 'mean_word_length': 3.3325116406463984, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 18, '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': '5810962', 'n_tokens_mistral': 6706, 'n_tokens_neox': 6301, 'n_words': 1156}