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
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 = []
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]
