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 __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,
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 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]
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: "
# 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 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 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'))
