def preprocess_all_gps_data(): name2bb = pickle.load(open('/scail/group/deeplearning/sail-deep-gpu/imagenet_dicts/driving_name2bb.pkl')) lane_prefix = '/scail/group/deeplearning/driving_data/nikhilb/recovered_lane_labels/' + \ '_scail_group_deeplearning_driving_data_twangcat_raw_data_backup_' gps_prefix = '/scail/group/deeplearning/driving_data/twangcat/raw_data_backup/' lane_prefix = '/scail/group/deeplearning/driving_data/nikhilb/recovered_lane_labels/' + \ '_scail_group_deeplearning_driving_data_twangcat_raw_data_backup_' gps_data = {} for name, bb_info in sorted(name2bb.iteritems()): id = get_id(name) if id in gps_data: continue gps_file_prefix = id.split('-')[-1] gps_filename = gps_file_prefix + '_gps.out' folder_name = get_folder_name(id) try: gps_reader = GPSReader(gps_prefix + folder_name + '/' + gps_filename) except: print 'Skipped:', id continue gps_data[id] = {'gps_data' : gps_reader.getNumericData()} gps_data[id]['gps_data'][:, [4, 5]] = gps_data[id]['gps_data'][:, [5, 4]] ov1, ov2 = get_orthogonal_velocity(gps_data[id]['gps_data']) for camIdx in xrange(1, 3): cam = getCameraParams()[camIdx - 1] tr = GPSTransforms(gps_data[id]['gps_data'], cam) lane_path = lane_prefix + folder_name.replace('/', '_') + '-' + gps_file_prefix + \ str(camIdx) + '-interpolated.mat' try: lane_labels = loadmat(lane_path) except: print 'No lane labels:', lane_path continue lp = lane_labels['left'] rp = lane_labels['right'] Tc = np.eye(4) left_XYZ = pixelTo3d(lp, cam) right_XYZ = pixelTo3d(rp, cam) left_Pos = np.zeros((lp.shape[0], 4)) right_Pos = np.zeros((rp.shape[0], 4)) ov1_Pos = np.ones((ov1.shape[0], 4)) ov2_Pos = np.ones((ov2.shape[0], 4)) ov1_Pos[:, 0:3] = GPSPos(ov1, cam, gps_data[id]['gps_data'][0, :]).transpose() ov2_Pos[:, 0:3] = GPSPos(ov2, cam, gps_data[id]['gps_data'][0, :]).transpose() for t in range(min(lp.shape[0], tr.shape[0])): left_Pos[t,:] = np.dot(tr[t, :, :], np.linalg.solve(Tc, np.array([left_XYZ[t,0], left_XYZ[t,1], left_XYZ[t,2], 1]))) right_Pos[t,:] = np.dot(tr[t, :, :], np.linalg.solve(Tc, np.array([right_XYZ[t,0], right_XYZ[t,1], right_XYZ[t,2], 1]))) gps_data[id]['tr' + str(camIdx)] = tr gps_data[id]['left_Pos' + str(camIdx)] = left_Pos gps_data[id]['right_Pos' + str(camIdx)] = right_Pos gps_data[id]['ov1_Pos' + str(camIdx)] = ov1_Pos gps_data[id]['ov2_Pos' + str(camIdx)] = ov2_Pos print len(gps_data), 'gps files pre-processed' pickle.dump(gps_data, open(preprocessed_gps_data_dir + 'preprocessed_gps_data_using_velocity.pkl', 'w'))
def runLabeling(file_path, gps_filename, output_name, frames_to_skip, final_frame, lp, rp, pickle_loc): video_reader = VideoReader(file_path) video_reader.setSubsample(True) #video_reader.setPerspectives(pickle_loc) gps_reader = GPSReader(gps_filename) gps_dat = gps_reader.getNumericData() cam = getCameraParams() cam_to_use = cam[int(output_name[-1]) - 1] lp = pixelTo3d(lp, cam_to_use) rp = pixelTo3d(rp, cam_to_use) tr = GPSTransforms(gps_dat, cam_to_use) pitch = -cam_to_use['rot_x'] height = 1.106 R_camera_pitch = euler_matrix(cam_to_use['rot_x'], cam_to_use['rot_y'], cam_to_use['rot_z'], 'sxyz')[0:3, 0:3] Tc = np.eye(4) Tc[0:3, 0:3] = R_camera_pitch.transpose() Tc[0:3, 3] = [-0.2, -height, -0.5] lpts = np.zeros((lp.shape[0], 4)) rpts = np.zeros((rp.shape[0], 4)) for t in range(min(tr.shape[0], lp.shape[0])): lpts[t, :] = np.dot(tr[t, :, :], np.linalg.solve(Tc, np.array([lp[t, 0], lp[t, 1], lp[t, 2], 1]))) rpts[t, :] = np.dot(tr[t, :, :], np.linalg.solve(Tc, np.array([rp[t, 0], rp[t, 1], rp[t, 2], 1]))) ldist = np.apply_along_axis(np.linalg.norm, 1, np.concatenate((np.array([[0, 0, 0, 0]]), lpts[1:] - lpts[0:-1]))) rdist = np.apply_along_axis(np.linalg.norm, 1, np.concatenate((np.array([[0, 0, 0, 0]]), rpts[1:] - rpts[0:-1]))) start_frame = frames_to_skip runBatch(video_reader, gps_dat, cam_to_use, output_name, start_frame, final_frame, lpts, rpts, ldist, rdist, tr) print "Done with %s" % output_name
def runLabeling(file_path, gps_filename, output_name, frames_to_skip, final_frame, lp, rp, pickle_loc): video_reader = WarpedVideoReader(file_path) #video_reader.setSubsample(True) video_reader.setPerspectives(pickle_loc) gps_reader = GPSReader(gps_filename) gps_dat = gps_reader.getNumericData() cam = getCameraParams() cam_to_use = cam[int(output_name[-1]) - 1] lp = pixelTo3d(lp, cam_to_use) rp = pixelTo3d(rp, cam_to_use) tr = GPSTransforms(gps_dat, cam_to_use) pitch = -cam_to_use['rot_x'] height = 1.106 R_camera_pitch = euler_matrix(cam_to_use['rot_x'], cam_to_use['rot_y'], cam_to_use['rot_z'], 'sxyz')[0:3, 0:3] Tc = np.eye(4) Tc[0:3, 0:3] = R_camera_pitch.transpose() Tc[0:3, 3] = [-0.2, -height, -0.5] lpts = np.zeros((lp.shape[0], 4)) rpts = np.zeros((rp.shape[0], 4)) for t in range(min(tr.shape[0], lp.shape[0])): lpts[t, :] = np.dot(tr[t, :, :], np.linalg.solve(Tc, np.array([lp[t, 0], lp[t, 1], lp[t, 2], 1]))) rpts[t, :] = np.dot(tr[t, :, :], np.linalg.solve(Tc, np.array([rp[t, 0], rp[t, 1], rp[t, 2], 1]))) ldist = np.apply_along_axis(np.linalg.norm, 1, np.concatenate((np.array([[0, 0, 0, 0]]), lpts[1:] - lpts[0:-1]))) rdist = np.apply_along_axis(np.linalg.norm, 1, np.concatenate((np.array([[0, 0, 0, 0]]), rpts[1:] - rpts[0:-1]))) start_frame = frames_to_skip runBatch(video_reader, gps_dat, cam_to_use, output_name, start_frame, final_frame, lpts, rpts, ldist, rdist, tr) print "Done with %s" % output_name
def setupParams(self, root,f): args = parse_args(root, f+'.avi') params = args['params'] # 50Hz gps data gps_name = args['gps_mark1'] print gps_name gps_reader = GPSReader(gps_name) GPSData = gps_reader.getNumericData() if GPSData.shape[0] ==0: print 'empty gps1 log. skipping...' return False self.imuTransforms = IMUTransforms(GPSData) self.GPSTime = utc_from_gps_log_all(GPSData) # Video-synced gps data gps_name2 = args['gps_mark2'] gps_reader2 = GPSReader(gps_name2) GPSData2 = gps_reader2.getNumericData() if GPSData2.shape[0] ==0: print 'empty gps2 log. skipping...' return False self.imuTransforms2 = IMUTransforms(GPSData2) # grab the initial time off the gps log and compute start and end times self.start_time = self.GPSTime[0] + 1 * 1e6 self.end_time = self.GPSTime[0] + 600 * 1e6 if self.end_time > self.GPSTime[-1]: self.end_time = self.GPSTime[-1] self.step_time = 0.5 self.scan_window = 0.1 self.lidar_loader = LDRLoader(args['frames']) self.T_from_l_to_i = params['lidar']['T_from_l_to_i'] self.lidar_height = params['lidar']['height'] print 'gps: '+gps_name map_name = gps_name[0:-self.name_offset]+'.map' print 'map: '+ map_name self.map_outname = os.path.join(self.targetfolder, (gps_name[0:-self.name_offset]+'_lidarmap.pickle')) self.lane_outname = os.path.join(self.targetfolder, (gps_name[0:-self.name_offset]+'_interp_lanes.pickle')) print 'out: '+ self.lane_outname if os.path.isfile(self.lane_outname): print self.lane_outname+' already exists, skipping...' return False total_num_frames = GPSData.shape[0] velocities = GPSData[:,4:7] velocities[:,[0, 1]] = velocities[:,[1, 0]] vel_start = ENU2IMUQ50(np.transpose(velocities), GPSData[0,:]) # sideways vector wrt starting imu frame sideways_start = np.cross(vel_start.transpose(), self.imuTransforms[:,0:3,2], axisa=1, axisb=1, axisc=1) self.sideways = sideways_start/(np.sqrt((sideways_start ** 2).sum(1))[...,np.newaxis]) # normalize to unit vector # initialize empty data holders self.left_data = [ ] self.right_data = [ ] self.left_time = [ ] self.right_time = [ ] self.all_data = dict() self.all_time = dict() return True
def __init__(self, args, start_time, end_time, step_time, scan_window, absolute=False): """ start_time: seconds into the GPS Mark 1 log end_time: seconds into the GPS Mark 1 log step_time: seconds between adding points from scans scan_window: +/- seconds of data to add between each step """ self.args = args self.params = self.args['params'] self.gps_reader_mark1 = GPSReader(self.args['gps_mark1']) self.gps_data_mark1 = self.gps_reader_mark1.getNumericData() self.lidar_loader = LDRLoader(self.args['frames']) if absolute: self.imu_transforms_mark1 = absoluteTransforms(self.gps_data_mark1) else: self.imu_transforms_mark1 = IMUTransforms(self.gps_data_mark1) self.gps_times_mark1 = utc_from_gps_log_all(self.gps_data_mark1) self.T_from_l_to_i = self.params['lidar']['T_from_l_to_i'] self.T_from_i_to_l = np.linalg.inv(self.T_from_l_to_i) # grab the initial time off the gps log and compute start and end times self.start_time = self.gps_times_mark1[0] + start_time * 1e6 self.end_time = self.gps_times_mark1[0] + end_time * 1e6 if self.end_time > self.gps_times_mark1[-1]: self.end_time = self.gps_times_mark1[-1] self.step_time = step_time self.scan_window = scan_window self.all_data = [] self.all_t = []
def __init__(self, args, npz_file): self.map_file = args['map'] self.lidar_params = args['params']['lidar'] self.radar_params = args['params']['radar'] self.radar_actors = [] self.rdr_pts = loadRDRCamMap(self.map_file) self.count = 0 self.lidar_actor = None self.lidar_data = np.load(npz_file)['data'] gps_filename = args['gps'] gps_reader = GPSReader(gps_filename) GPSData = gps_reader.getNumericData() self.imu_transforms = IMUTransforms(GPSData)
def __init__(self, single_record_dir, magnetometer_calibration): threading.Thread.__init__(self) self.set_config('config/position.cfg') self.imu = IMUReader.ImuReader(magnetometer_calibration, self.imu_read_freq, self.obj_x, self.obj_y, self.obj_z, self.reverse) self.gps = GPSReader.GpsReader(self.gps_read_freq) self.imu.start() self.gps.start() if self.to_stdout: self.output = open('/dev/stdout', "wb") else: self.output = open(self.path + single_record_dir + self.filename, "wb") self.writer = csv.writer(self.output, delimiter='\t', quotechar='"', quoting=csv.QUOTE_NONNUMERIC) self.running = False self.gps_fixed = False self.imu_data = NaN self.imu_data_old = NaN self.gps_data = NaN self.gps_data_old = NaN self.imu_changed = False self.gps_changed = False self.speed = 0.0 self.climb = 0 self.latitude = 0 self.longitude = 0 self.altitude = 0 self.pressure = 0 self.pressure_ref = 0 self.gyro_scaled_x = 0 self.gyro_scaled_y = 0 self.gyro_scaled_z = 0 self.accel_scaled_x = 0 self.accel_scaled_y = 0 self.accel_scaled_z = 0 self.temperature = 0 self.track = 0 self.mode = 0 self.nb_sats = 0 self.pitch = 0 self.roll = 0 self.yaw = 0
class MapBuilder(object): def __init__(self, args, start_time, end_time, step_time, scan_window, absolute=False): """ start_time: seconds into the GPS Mark 1 log end_time: seconds into the GPS Mark 1 log step_time: seconds between adding points from scans scan_window: +/- seconds of data to add between each step """ self.args = args self.params = self.args['params'] self.gps_reader_mark1 = GPSReader(self.args['gps_mark1']) self.gps_data_mark1 = self.gps_reader_mark1.getNumericData() self.lidar_loader = LDRLoader(self.args['frames']) if absolute: self.imu_transforms_mark1 = absoluteTransforms(self.gps_data_mark1) else: self.imu_transforms_mark1 = IMUTransforms(self.gps_data_mark1) self.gps_times_mark1 = utc_from_gps_log_all(self.gps_data_mark1) self.T_from_l_to_i = self.params['lidar']['T_from_l_to_i'] self.T_from_i_to_l = np.linalg.inv(self.T_from_l_to_i) # grab the initial time off the gps log and compute start and end times self.start_time = self.gps_times_mark1[0] + start_time * 1e6 self.end_time = self.gps_times_mark1[0] + end_time * 1e6 if self.end_time > self.gps_times_mark1[-1]: self.end_time = self.gps_times_mark1[-1] self.step_time = step_time self.scan_window = scan_window self.all_data = [] self.all_t = [] def buildMap(self, filters=None): """ Creates a map with a set of filters filters: A list of filters. 'ground': Leaves the ground points 'lanes': Filters out the lane markings 'forward': Only takes the top half of the scan. This ensures points appear in chronolgical order 'flat': Makes all x distances 0. This makes lane clustering better """ self.all_data = [] self.all_t = [] current_time = self.start_time print (self.end_time - current_time) / 1e6 while current_time + self.scan_window < self.end_time: # load points w.r.t lidar at current time data, t_data = self.lidar_loader.loadLDRWindow(current_time, self.scan_window) if data is None or data.shape[0] == 0: current_time += self.step_time * 1e6 continue if filters != None: if 'ground' in filters: filter_mask = data[:, 3] < 10 else: filter_mask = data[:, 3] > 30 if 'lanes' in filters: filter_mask &= (data[:,1] < 3) & (data [:,1] > -3) &\ (data[:,2] < -1.9) & (data[:,2] > -2.1) if 'forward' in filters: filter_mask &= (data[:, 0] > 0) if 'no-trees' in filters: filter_mask &= (data[:,1] < 30) & (data [:,1] > -30) &\ (data[:,2] < -1) & (data[:,2] > -3) if 'flat' in filters: data[:, 0] = 0 data = data[filter_mask, :] t_data = t_data[filter_mask] # put in imu_t frame pts = data[:, 0:3].transpose() pts = np.vstack((pts, np.ones((1, pts.shape[1])))) pts = np.dot(self.T_from_l_to_i, pts) # Shift points according to timestamps instead of using # transform of full sweep. # This will put pts in imu_0 frame transform_points_by_times(pts, t_data, self.imu_transforms_mark1, self.gps_times_mark1) data[:, 0:3] = pts[0:3, :].transpose() self.all_data.append(data.copy()) self.all_t.append(t_data.copy()) current_time += self.step_time * 1e6 print (self.end_time - current_time) / 1e6 def getData(self): export_data = np.row_stack(self.all_data) export_t = np.concatenate(self.all_t) return (export_data, export_t) def exportData(self, file_name): print 'exporting data' export_data = np.row_stack(self.all_data) export_t = np.concatenate(self.all_t) print export_data print export_data.shape print export_t.shape np.savez(file_name, data=export_data, t=export_t) print 'export complete'
def trackbarOnchange(t, prev_t): if abs(t - prev_t) > 1: video_reader.setFrame(t) if __name__ == '__main__': args = parse_args(sys.argv[1], sys.argv[2]) cam_num = int(sys.argv[2][-5]) video_file = args['video'] params = args['params'] cam = params['cam'][cam_num - 1] video_reader = VideoReader(video_file) gps_reader = GPSReader(args['gps']) GPSData = gps_reader.getNumericData() imu_transforms = IMUTransforms(GPSData) T_from_i_to_l = np.linalg.inv(params['lidar']['T_from_l_to_i']) all_data = np.load(sys.argv[3]) map_data = all_data['data'] #map_data = map_data[map_data[:,3] > 60, :] # map points are defined w.r.t the IMU position at time 0 # each entry in map_data is (x,y,z,intensity,framenum). print "Hit 'q' to quit" trackbarInit = False interp_grid = np.mgrid[0:IMG_WIDTH, 0:IMG_HEIGHT]
class MapBuilder(object): def __init__(self, args, start_time, end_time, step_time, scan_window, absolute=False): """ start_time: seconds into the GPS Mark 1 log end_time: seconds into the GPS Mark 1 log step_time: seconds between adding points from scans scan_window: +/- seconds of data to add between each step """ self.args = args self.params = self.args['params'] self.gps_reader_mark1 = GPSReader(self.args['gps_mark1']) self.gps_data_mark1 = self.gps_reader_mark1.getNumericData() self.lidar_loader = LDRLoader(self.args['frames']) if absolute: self.imu_transforms_mark1 = absoluteTransforms(self.gps_data_mark1) else: self.imu_transforms_mark1 = IMUTransforms(self.gps_data_mark1) self.gps_times_mark1 = utc_from_gps_log_all(self.gps_data_mark1) self.T_from_l_to_i = self.params['lidar']['T_from_l_to_i'] self.T_from_i_to_l = np.linalg.inv(self.T_from_l_to_i) # grab the initial time off the gps log and compute start and end times self.start_time = self.gps_times_mark1[0] + start_time * 1e6 self.end_time = self.gps_times_mark1[0] + end_time * 1e6 if self.end_time > self.gps_times_mark1[-1]: self.end_time = self.gps_times_mark1[-1] self.step_time = step_time self.scan_window = scan_window self.all_data = [] self.all_t = [] def buildMap(self, filters=None): """ Creates a map with a set of filters filters: A list of filters. 'ground': Leaves the ground points 'lanes': Filters out the lane markings 'forward': Only takes the top half of the scan. This ensures points appear in chronolgical order 'flat': Makes all x distances 0. This makes lane clustering better """ self.all_data = [] self.all_t = [] current_time = self.start_time print(self.end_time - current_time) / 1e6 while current_time + self.scan_window < self.end_time: # load points w.r.t lidar at current time data, t_data = self.lidar_loader.loadLDRWindow( current_time, self.scan_window) if data is None: current_time += self.step_time * 1e6 continue if filters != None: if 'ground' in filters: filter_mask = data[:, 3] < 10 else: filter_mask = data[:, 3] > 30 if 'lanes' in filters: filter_mask &= (data[:,1] < 3) & (data [:,1] > -3) &\ (data[:,2] < -1.9) & (data[:,2] > -2.1) if 'forward' in filters: filter_mask &= (data[:, 0] > 0) if 'no-trees' in filters: filter_mask &= (data[:,1] < 30) & (data [:,1] > -30) &\ (data[:,2] < -1) & (data[:,2] > -3) if 'flat' in filters: data[:, 0] = 0 data = data[filter_mask, :] t_data = t_data[filter_mask] # put in imu_t frame pts = data[:, 0:3].transpose() pts = np.vstack((pts, np.ones((1, pts.shape[1])))) pts = np.dot(self.T_from_l_to_i, pts) # Shift points according to timestamps instead of using # transform of full sweep. # This will put pts in imu_0 frame transform_points_by_times(pts, t_data, self.imu_transforms_mark1, self.gps_times_mark1) data[:, 0:3] = pts[0:3, :].transpose() self.all_data.append(data.copy()) self.all_t.append(t_data.copy()) current_time += self.step_time * 1e6 print(self.end_time - current_time) / 1e6 def getData(self): export_data = np.row_stack(self.all_data) export_t = np.concatenate(self.all_t) return (export_data, export_t) def exportData(self, file_name): print 'exporting data' export_data = np.row_stack(self.all_data) export_t = np.concatenate(self.all_t) print export_data print export_data.shape print export_t.shape np.savez(file_name, data=export_data, t=export_t) print 'export complete'
return (pix, mask) if __name__ == "__main__": args = parse_args(sys.argv[1], sys.argv[2]) cam_num = args["cam_num"] video_file = args["video"] video_reader = VideoReader(video_file) params = args["params"] cam = params["cam"][cam_num] all_data = np.load(sys.argv[3]) map_data = all_data["data"] map_t = all_data["t"] gps_reader_mark1 = GPSReader(args["gps_mark1"]) gps_data_mark1 = gps_reader_mark1.getNumericData() gps_reader_mark2 = GPSReader(args["gps_mark2"]) gps_data_mark2 = gps_reader_mark2.getNumericData() imu_transforms_mark1 = IMUTransforms(gps_data_mark1) imu_transforms_mark2 = IMUTransforms(gps_data_mark2) gps_times_mark1 = utc_from_gps_log_all(gps_data_mark1) gps_times_mark2 = utc_from_gps_log_all(gps_data_mark2) 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: (success, I) = video_reader.getNextFrame() fnum = video_reader.framenum
video_file = args['video'] video_reader = VideoReader(video_file) params = args['params'] cam = params['cam'][cam_num-1] if os.path.isfile(args['gps_mark2']): gps_key1='gps_mark1' gps_key2='gps_mark2' postfix_len = 13 else: gps_key1='gps' gps_key2='gps' postfix_len=8 #gps_filename= args['gps'] gps_filename= args[gps_key2] gps_reader = GPSReader(gps_filename) prefix = gps_filename[0:-postfix_len] gps_data = gps_reader.getNumericData() lidar_loader = LDRLoader(args['frames']) imu_transforms = IMUTransforms(gps_data) gps_times = utc_from_gps_log_all(gps_data) gps_filename1= args[gps_key1] gps_reader1 = GPSReader(gps_filename1) gps_data1 = gps_reader1.getNumericData() imu_transforms1 = IMUTransforms(gps_data1) gps_times1 = utc_from_gps_log_all(gps_data1) lane_filename = string.replace(prefix+'_multilane_points_done.npz', 'q50_data', '640x480_Q50') lanes = np.load(lane_filename)
yaw_0).transpose() enu_wrt_imu_0 = np.dot(T_to_w_from_i, pos_wrt_imu_0).transpose() # ENU w.r.t imu at time 0 -> WGS84 wgs84_pts = ENUtoWGS84(gps_dat[0, 1:4], enu_wrt_imu_0[:, 0:3]).transpose() return wgs84_pts if __name__ == '__main__': video_filename = sys.argv[1] path, vfname = os.path.split(video_filename) vidname = vfname.split('.')[0] cam_num = int(vidname[-1]) gps_filename = path + '/' + vidname[0:-1] + '_gps.out' video_reader = VideoReader(video_filename, num_splits=1) gps_reader = GPSReader(gps_filename) gps_dat = gps_reader.getNumericData() cam = getCameraParams()[cam_num - 1] labels = loadmat(sys.argv[2]) lp = labels['left'] rp = labels['right'] l_gps_coord = pixelToGPS(lp, gps_dat, cam) r_gps_coord = pixelToGPS(rp, gps_dat, cam) print "gps_dat: " print gps_dat[:, 1:4] print "l_gps_dat: " print l_gps_coord print "r_gps_dat: "
T_to_w_from_i[0:3, 0:3] = R_to_i_from_w(roll_0, pitch_0, yaw_0).transpose() enu_wrt_imu_0 = np.dot(T_to_w_from_i, pos_wrt_imu_0).transpose() # ENU w.r.t imu at time 0 -> WGS84 wgs84_pts = ENUtoWGS84(gps_dat[0, 1:4], enu_wrt_imu_0[:, 0:3]).transpose() return wgs84_pts if __name__ == "__main__": video_filename = sys.argv[1] path, vfname = os.path.split(video_filename) vidname = vfname.split(".")[0] cam_num = int(vidname[-1]) gps_filename = path + "/" + vidname[0:-1] + "_gps.out" video_reader = VideoReader(video_filename, num_splits=1) gps_reader = GPSReader(gps_filename) gps_dat = gps_reader.getNumericData() cam = getCameraParams()[cam_num - 1] labels = loadmat(sys.argv[2]) lp = labels["left"] rp = labels["right"] l_gps_coord = pixelToGPS(lp, gps_dat, cam) r_gps_coord = pixelToGPS(rp, gps_dat, cam) print "gps_dat: " print gps_dat[:, 1:4] print "l_gps_dat: " print l_gps_coord print "r_gps_dat: "
def preprocess_all_gps_data(): name2bb = pickle.load( open( '/scail/group/deeplearning/sail-deep-gpu/imagenet_dicts/driving_name2bb.pkl' )) lane_prefix = '/scail/group/deeplearning/driving_data/nikhilb/recovered_lane_labels/' + \ '_scail_group_deeplearning_driving_data_twangcat_raw_data_backup_' gps_prefix = '/scail/group/deeplearning/driving_data/twangcat/raw_data_backup/' lane_prefix = '/scail/group/deeplearning/driving_data/nikhilb/recovered_lane_labels/' + \ '_scail_group_deeplearning_driving_data_twangcat_raw_data_backup_' gps_data = {} for name, bb_info in sorted(name2bb.iteritems()): id = get_id(name) if id in gps_data: continue gps_file_prefix = id.split('-')[-1] gps_filename = gps_file_prefix + '_gps.out' folder_name = get_folder_name(id) try: gps_reader = GPSReader(gps_prefix + folder_name + '/' + gps_filename) except: print 'Skipped:', id continue gps_data[id] = {'gps_data': gps_reader.getNumericData()} gps_data[id]['gps_data'][:, [4, 5]] = gps_data[id]['gps_data'][:, [5, 4]] ov1, ov2 = get_orthogonal_velocity(gps_data[id]['gps_data']) for camIdx in xrange(1, 3): cam = getCameraParams()[camIdx - 1] tr = GPSTransforms(gps_data[id]['gps_data'], cam) lane_path = lane_prefix + folder_name.replace('/', '_') + '-' + gps_file_prefix + \ str(camIdx) + '-interpolated.mat' try: lane_labels = loadmat(lane_path) except: print 'No lane labels:', lane_path continue lp = lane_labels['left'] rp = lane_labels['right'] Tc = np.eye(4) left_XYZ = pixelTo3d(lp, cam) right_XYZ = pixelTo3d(rp, cam) left_Pos = np.zeros((lp.shape[0], 4)) right_Pos = np.zeros((rp.shape[0], 4)) ov1_Pos = np.ones((ov1.shape[0], 4)) ov2_Pos = np.ones((ov2.shape[0], 4)) ov1_Pos[:, 0:3] = GPSPos(ov1, cam, gps_data[id]['gps_data'][0, :]).transpose() ov2_Pos[:, 0:3] = GPSPos(ov2, cam, gps_data[id]['gps_data'][0, :]).transpose() for t in range(min(lp.shape[0], tr.shape[0])): left_Pos[t, :] = np.dot( tr[t, :, :], np.linalg.solve( Tc, np.array([ left_XYZ[t, 0], left_XYZ[t, 1], left_XYZ[t, 2], 1 ]))) right_Pos[t, :] = np.dot( tr[t, :, :], np.linalg.solve( Tc, np.array([ right_XYZ[t, 0], right_XYZ[t, 1], right_XYZ[t, 2], 1 ]))) gps_data[id]['tr' + str(camIdx)] = tr gps_data[id]['left_Pos' + str(camIdx)] = left_Pos gps_data[id]['right_Pos' + str(camIdx)] = right_Pos gps_data[id]['ov1_Pos' + str(camIdx)] = ov1_Pos gps_data[id]['ov2_Pos' + str(camIdx)] = ov2_Pos print len(gps_data), 'gps files pre-processed' pickle.dump( gps_data, open( preprocessed_gps_data_dir + 'preprocessed_gps_data_using_velocity.pkl', 'w'))
def setupParams(self, root, f): args = parse_args(root, f + '.avi') params = args['params'] # 50Hz gps data gps_name = args['gps_mark1'] print gps_name gps_reader = GPSReader(gps_name) GPSData = gps_reader.getNumericData() if GPSData.shape[0] == 0: print 'empty gps1 log. skipping...' return False self.imuTransforms = IMUTransforms(GPSData) self.GPSTime = utc_from_gps_log_all(GPSData) # Video-synced gps data gps_name2 = args['gps_mark2'] gps_reader2 = GPSReader(gps_name2) GPSData2 = gps_reader2.getNumericData() if GPSData2.shape[0] == 0: print 'empty gps2 log. skipping...' return False self.imuTransforms2 = IMUTransforms(GPSData2) # grab the initial time off the gps log and compute start and end times self.start_time = self.GPSTime[0] + 1 * 1e6 self.end_time = self.GPSTime[0] + 600 * 1e6 if self.end_time > self.GPSTime[-1]: self.end_time = self.GPSTime[-1] self.step_time = 0.5 self.scan_window = 0.1 self.lidar_loader = LDRLoader(args['frames']) self.T_from_l_to_i = params['lidar']['T_from_l_to_i'] self.lidar_height = params['lidar']['height'] print 'gps: ' + gps_name map_name = gps_name[0:-self.name_offset] + '.map' print 'map: ' + map_name self.map_outname = os.path.join( self.targetfolder, (gps_name[0:-self.name_offset] + '_lidarmap.pickle')) self.lane_outname = os.path.join( self.targetfolder, (gps_name[0:-self.name_offset] + '_interp_lanes.pickle')) print 'out: ' + self.lane_outname if os.path.isfile(self.lane_outname): print self.lane_outname + ' already exists, skipping...' return False total_num_frames = GPSData.shape[0] velocities = GPSData[:, 4:7] velocities[:, [0, 1]] = velocities[:, [1, 0]] vel_start = ENU2IMUQ50(np.transpose(velocities), GPSData[0, :]) # sideways vector wrt starting imu frame sideways_start = np.cross(vel_start.transpose(), self.imuTransforms[:, 0:3, 2], axisa=1, axisb=1, axisc=1) self.sideways = sideways_start / (np.sqrt( (sideways_start**2).sum(1))[..., np.newaxis] ) # normalize to unit vector # initialize empty data holders self.left_data = [] self.right_data = [] self.left_time = [] self.right_time = [] self.all_data = dict() self.all_time = dict() return True
# 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 = args['cam_num'] video_file = args['video'] video_reader = VideoReader(video_file) params = args['params'] cam = params['cam'][cam_num] gps_reader_mark1 = GPSReader(args['gps_mark1']) gps_data_mark1 = gps_reader_mark1.getNumericData() gps_reader_mark2 = GPSReader(args['gps_mark2']) gps_data_mark2 = gps_reader_mark2.getNumericData() imu_transforms_mark1 = IMUTransforms(gps_data_mark1) imu_transforms_mark2 = IMUTransforms(gps_data_mark2) gps_times_mark1 = utc_from_gps_log_all(gps_data_mark1) gps_times_mark2 = utc_from_gps_log_all(gps_data_mark2) builder = MapBuilder(args, 0, 999999, 0.1, 0.05) # parameter server thr = ThreadedServer(port) thr.setDaemon(True) thr.start() time.sleep(1)
video_file = args['video'] video_reader = VideoReader(video_file) params = args['params'] cam = params['cam'][cam_num - 1] if os.path.isfile(args['gps_mark2']): gps_key1 = 'gps_mark1' gps_key2 = 'gps_mark2' postfix_len = 13 else: gps_key1 = 'gps' gps_key2 = 'gps' postfix_len = 8 #gps_filename= args['gps'] gps_filename = args[gps_key2] gps_reader = GPSReader(gps_filename) prefix = gps_filename[0:-postfix_len] gps_data = gps_reader.getNumericData() lidar_loader = LDRLoader(args['frames']) imu_transforms = IMUTransforms(gps_data) gps_times = utc_from_gps_log_all(gps_data) gps_filename1 = args[gps_key1] gps_reader1 = GPSReader(gps_filename1) gps_data1 = gps_reader1.getNumericData() imu_transforms1 = IMUTransforms(gps_data1) gps_times1 = utc_from_gps_log_all(gps_data1) lane_filename = string.replace(prefix + '_multilane_points_done.npz', 'q50_data', '640x480_Q50') lanes = np.load(lane_filename)
def trackbarOnchange(t, prev_t): if abs(t - prev_t) > 1: video_reader.setFrame(t) if __name__ == '__main__': args = parse_args(sys.argv[1], sys.argv[2]) cam_num = int(sys.argv[2][-5]) video_file = args['video'] params = args['params'] cam = params['cam'][cam_num-1] video_reader = VideoReader(video_file) gps_reader = GPSReader(args['gps']) GPSData = gps_reader.getNumericData() imu_transforms = IMUTransforms(GPSData) T_from_i_to_l = np.linalg.inv(params['lidar']['T_from_l_to_i']) all_data = np.load(sys.argv[3]) map_data = all_data['data'] #map_data = map_data[map_data[:,3] > 60, :] # map points are defined w.r.t the IMU position at time 0 # each entry in map_data is (x,y,z,intensity,framenum). print "Hit 'q' to quit" trackbarInit = False interp_grid = np.mgrid[0:IMG_WIDTH, 0:IMG_HEIGHT]