def get_transforms(folder, run, mark='mark1'):
""" Gets the IMU transforms for a run """
gps_reader = GPSReader(folder + run + '_gps' + mark + '.out')
gps_data = gps_reader.getNumericData()
gps_times = utc_from_gps_log_all(gps_data)
imu_transforms = IMUTransforms(gps_data)
return imu_transforms, gps_times
def export(datafile):
f = GPSReader(datafile)
d = f.getNumericData()
out = datafile.split('raw_data')
outname = out[1][1:].replace('/','_')
outname = outname.replace('.out', '.mat')
savemat(outname, dict(data=d));
def get_transforms(folder, run, mark = 'mark1'):
""" Gets the IMU transforms for a run """
gps_reader = GPSReader(folder + run + '_gps' + mark + '.out')
gps_data = gps_reader.getNumericData()
gps_times = utc_from_gps_log_all(gps_data)
imu_transforms = IMUTransforms(gps_data)
return imu_transforms, gps_times
def get_transforms(args, mark='mark1'):
""" Gets the IMU transforms for a run """
gps_reader = GPSReader(args['gps_' + mark])
gps_data = gps_reader.getNumericData()
gps_times = utc_from_gps_log_all(gps_data)
imu_transforms = IMUTransforms(gps_data)
return imu_transforms, gps_data, gps_times
def get_transforms(args, mark='mark1'):
""" Gets the IMU transforms for a run """
gps_reader = GPSReader(args['gps_' + mark])
gps_data = gps_reader.getNumericData()
gps_times = utc_from_gps_log_all(gps_data)
imu_transforms = IMUTransforms(gps_data)
return imu_transforms, gps_data, gps_times
def read_gps_fields(gps_file, fields):
gps_reader = GPSReader(gps_file)
data = gps_reader.getNumericData()
field_data = list()
for field in fields:
field_data.append(data[:, gps_reader.token_order.index(field)])
return field_data
def read_gps_fields(gps_file, fields):
gps_reader = GPSReader(gps_file)
data = gps_reader.getNumericData()
field_data = list()
for field in fields:
field_data.append(data[:, gps_reader.token_order.index(field)])
return field_data
def numseconds(datafile):
f = GPSReader(datafile)
d = f.getData()
try:
first = d[0]['seconds']
last = d[-1]['seconds']
return last - first
except:
return 0
def main():
# CANReader.start_thread()
GPSReader.start_thread()
GAReader.start_thread()
AVIReader.start_thread()
signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C')
forever = threading.Event()
forever.wait()
def numseconds(datafile):
f = GPSReader(datafile)
d = f.getData()
try:
first = d[0]['seconds']
last = d[-1]['seconds']
return last - first
except:
return 0
def runLabelling(self, f, frames, Pid): # filename, frame numbers, transformation ids
Pid = Pid.tolist()
print frames
print Pid
cam_num = int(f[-5])
splitidx = string.index(f,'split_')
split_num = int(f[splitidx+6])
if split_num==0:
split_num=10
path, fname = os.path.split(f)
fname = fname[8:] # remove 'split_?'
args = parse_args(path, fname)
prefix = path + '/' + fname
params = args['params']
cam = params['cam'][cam_num-1]
self.label_scale = np.array([[float(self.labelw) / cam['width']], [float(self.labelh) / cam['height']]])
self.img_scale = np.array([[float(self.imwidth) / cam['width']], [float(self.imheight) / cam['height']]])
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_mark2
gps_filename2= args[gps_key2]
if not (gps_filename2 in self.gps_values2): # if haven't read this gps file before, cache it in dict.
gps_reader2 = GPSReader(gps_filename2)
self.gps_values2[gps_filename2] = gps_reader2.getNumericData()
gps_data2 = self.gps_values2[gps_filename2]
gps_times2 = utc_from_gps_log_all(gps_data2)
# gps_mark1
gps_filename1= args[gps_key1]
if not (gps_filename1 in self.gps_values1): # if haven't read this gps file before, cache it in dict.
gps_reader1 = GPSReader(gps_filename1)
self.gps_values1[gps_filename1] = gps_reader1.getNumericData()
gps_data1 = self.gps_values1[gps_filename1]
tr1 = IMUTransforms(gps_data1)
gps_times1 = utc_from_gps_log_all(gps_data1)
prefix = gps_filename2[0:-postfix_len]
#lane_filename = prefix+'_multilane_points_done.npz'
lane_filename = prefix+'_multilane_points_planar_done.npz'
if not (lane_filename in self.lane_values):
self.lane_values[lane_filename] = np.load(lane_filename)
lanes = self.lane_values[lane_filename] # these values are alread pre-computed and saved, now just read it from dictionary
start_frame = 0 #frames to skip #int(sys.argv[3]) if len(sys.argv) > 3 else 0
final_frame = -1 #int(sys.argv[4]) if len(sys.argv) > 4 else -1
return self.runBatch(f, gps_data1, gps_times1, gps_times2, frames, start_frame, final_frame, lanes, tr1, Pid, split_num, cam_num, params)
def __init__(self, gps_file, frames_folder, radar_bag_file, write_to_file):
if frames_folder[-1] == '/':
frames_folder = frames_folder[0:-1]
basename = frames_folder.replace('_frames', '')
self.map_file_name = basename + ".map"
reader = GPSReader(gps_file)
# Camera time should already be sorted because frame # always increases
camera_times = [
utc_from_gps(data['week'], data['seconds'])
for data in reader.getData()
]
ldr_times = \
[long(os.path.splitext(os.path.basename(ldr_file))[0])
for ldr_file in os.listdir(frames_folder)
if ldr_file.endswith('.ldr')]
# Need to sort the ldr_times because listdir returns undefined ordering
ldr_times.sort()
if EXPORT_RDR:
rdr_times = unpack_bag(basename, radar_bag_file)
for frame_number, camera_time in enumerate(camera_times):
# Find the closest time in ldr times
nearest_index_ldr = bisect.bisect(ldr_times, camera_time)
nearest_index_rdr = -1
if EXPORT_RDR:
nearest_index_rdr = bisect.bisect(rdr_times, camera_time)
if nearest_index_ldr >= 1 and (not EXPORT_RDR
or nearest_index_rdr >= 1):
lidr_file = str(ldr_times[nearest_index_ldr - 1]) + '.ldr'
if EXPORT_RDR:
radar_seq = str(rdr_times[nearest_index_rdr - 1]) + '.rdr'
# Frames are indexed by 1, not
real_frame = frame_number + 1
if EXPORT_RDR:
self.frame_to_cloud_map[real_frame] = (lidr_file,
radar_seq)
else:
self.frame_to_cloud_map[real_frame] = lidr_file
#print real_frame, (lidr_file, radar_seq)
if write_to_file:
self.__write_frame_map()
def __init__(self, gps_file, frames_folder, radar_bag_file, write_to_file):
if frames_folder[-1] == "/":
frames_folder = frames_folder[0:-1]
basename = frames_folder.replace("_frames", "")
self.map_file_name = basename + ".map"
reader = GPSReader(gps_file)
# Camera time should already be sorted because frame # always increases
camera_times = [utc_from_gps(data["week"], data["seconds"]) for data in reader.getData()]
ldr_times = [
long(os.path.splitext(os.path.basename(ldr_file))[0])
for ldr_file in os.listdir(frames_folder)
if ldr_file.endswith(".ldr")
]
# Need to sort the ldr_times because listdir returns undefined ordering
ldr_times.sort()
if EXPORT_RDR:
rdr_times = unpack_bag(basename, radar_bag_file)
for frame_number, camera_time in enumerate(camera_times):
# Find the closest time in ldr times
nearest_index_ldr = bisect.bisect(ldr_times, camera_time)
nearest_index_rdr = -1
if EXPORT_RDR:
nearest_index_rdr = bisect.bisect(rdr_times, camera_time)
if nearest_index_ldr >= 1 and (not EXPORT_RDR or nearest_index_rdr >= 1):
lidr_file = str(ldr_times[nearest_index_ldr - 1]) + ".ldr"
if EXPORT_RDR:
radar_seq = str(rdr_times[nearest_index_rdr - 1]) + ".rdr"
# Frames are indexed by 1, not
real_frame = frame_number + 1
if EXPORT_RDR:
self.frame_to_cloud_map[real_frame] = (lidr_file, radar_seq)
else:
self.frame_to_cloud_map[real_frame] = lidr_file
# print real_frame, (lidr_file, radar_seq)
if write_to_file:
self.__write_frame_map()
def get_transforms(args):
""" Gets the IMU transforms for a run """
gps_reader = GPSReader(args['gps'])
gps_data = gps_reader.getNumericData()
imu_transforms = IMUTransforms(gps_data)
return imu_transforms
Essentially just pieces from LidarIntegrator except avoids
storing the data for all time steps in memory
Writes full point clouds for scan matching later
'''
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Convert ldr files to h5 files containing points')
parser.add_argument('gps_file')
parser.add_argument('ldr_file')
parser.add_argument('h5_file')
parser.add_argument('--no_transform', action='store_true')
args = parser.parse_args()
if not args.no_transform:
gps_reader = GPSReader(args.gps_file)
gps_data = gps_reader.getNumericData()
#imu_transforms = IMUTransforms(gps_data)
imu_transforms = np.load(OPT_POS_FILE)['data']
params = LoadParameters(PARAMS_TO_LOAD)
# FIXME Assuming that ldr file named after frame num
fnum = int(os.path.splitext(os.path.basename(args.ldr_file))[0])
data = loadLDR(args.ldr_file)
if data.shape[0] == 0:
print '%d data empty' % fnum
raise
# Filter
# usage: # python test_gps_reader.py <path to gps log> import sys from GPSReader import GPSReader if __name__ == '__main__': reader = GPSReader(sys.argv[1]) print reader.getNumericData(); print reader.getNumericData()[:,1]
def get_transforms():
gps_reader = GPSReader(GPS_FILE)
gps_data = gps_reader.getNumericData()
imu_transforms = IMUTransforms(gps_data)
return imu_transforms
def read_imu_transforms(gps_file):
gps_reader = GPSReader(gps_file)
GPSData = gps_reader.getNumericData()
imu_transforms = IMUTransforms(GPSData)
return imu_transforms
from ArgParser import parse_args
from pipeline_config import LDR_DIR
from pipeline_config import EXPORT_START, EXPORT_NUM, EXPORT_STEP
'''
Create new lidar files synced to video
'''
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_mark2 = GPSReader(args['gps_mark2'])
gps_data_mark2 = gps_reader_mark2.getNumericData()
lidar_loader = LDRLoader(args['frames'])
imu_transforms_mark2 = IMUTransforms(gps_data_mark2)
gps_times_mark2 = utc_from_gps_log_all(gps_data_mark2)
frame_ldr_map = ''
# TODO Check this
if EXPORT_START != 0:
video_reader.setFrame(EXPORT_START)
count = 0
while count < EXPORT_NUM:
(success, I) = video_reader.getNextFrame()
def get_transforms(args):
""" Gets the IMU transforms for a run """
gps_reader = GPSReader(args['gps'])
gps_data = gps_reader.getNumericData()
imu_transforms = IMUTransforms(gps_data)
return imu_transforms
if __name__ == '__main__':
import os
import sys
import h5py
from ArgParser import parse_args
from GPSReader import GPSReader
from GPSTransforms import IMUTransforms
from pipeline_config import ICP_TRANSFORMS_DIR, EXPORT_START, EXPORT_NUM,\
EXPORT_STEP
args = parse_args(sys.argv[1], sys.argv[2])
# Load INS data
gps_reader = GPSReader(args['gps'])
GPSData = gps_reader.getNumericData()
imu_transforms = IMUTransforms(GPSData)
if '--full' in sys.argv:
T_start = 0
T_end = GPSData.shape[0] - GPSData.shape[0] % EXPORT_STEP
else:
T_start = EXPORT_START
T_end = T_start + EXPORT_NUM * EXPORT_STEP
nt = T_end - T_start
imu_states = imu_transforms[T_start:T_end, 0:3, 3]
INS_VAR = 1.0 # PARAM
SCAN_VAR = 0.1 # PARAM
from pipeline_config import EXPORT_START, EXPORT_NUM, EXPORT_STEP
'''
Create new lidar files synced to video
'''
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_mark2 = GPSReader(args['gps_mark2'])
gps_data_mark2 = gps_reader_mark2.getNumericData()
lidar_loader = LDRLoader(args['frames'])
imu_transforms_mark2 = IMUTransforms(gps_data_mark2)
gps_times_mark2 = utc_from_gps_log_all(gps_data_mark2)
frame_ldr_map = ''
# TODO Check this
if EXPORT_START != 0:
video_reader.setFrame(EXPORT_START)
count = 0
while count < EXPORT_NUM:
(success, I) = video_reader.getNextFrame()
storing the data for all time steps in memory
Writes full point clouds for scan matching later
'''
if __name__ == '__main__':
parser = argparse.ArgumentParser(
description='Convert ldr files to h5 files containing points')
parser.add_argument('gps_file')
parser.add_argument('ldr_file')
parser.add_argument('h5_file')
parser.add_argument('--no_transform', action='store_true')
args = parser.parse_args()
if not args.no_transform:
gps_reader = GPSReader(args.gps_file)
gps_data = gps_reader.getNumericData()
#imu_transforms = IMUTransforms(gps_data)
imu_transforms = np.load(OPT_POS_FILE)['data']
params = LoadParameters(PARAMS_TO_LOAD)
# FIXME Assuming that ldr file named after frame num
fnum = int(os.path.splitext(os.path.basename(args.ldr_file))[0])
data = loadLDR(args.ldr_file)
if data.shape[0] == 0:
print '%d data empty' % fnum
raise
# Filter
# Set as working directory
os.chdir(SavePath)
# Print current working directory
cwd = os.getcwd()
print(cwd)
# Logging for debugging purposes
logging.basicConfig(filename='app.log',
filemode='w',
format='%(name)s - %(levelname)s - %(message)s',
level=logging.INFO)
# CANReader = CANBusReader(CompleteCANFileName)
GPSReader = GPSReader(CompleteGPSFileName)
GAReader = GAReader(CompleteGAFileName)
AVIReader = AnalogReader(CompleteAVIFileName)
def signal_handler(signal, frame):
sys.exit(0)
def main():
# CANReader.start_thread()
GPSReader.start_thread()
GAReader.start_thread()
AVIReader.start_thread()
signal.signal(signal.SIGINT, signal_handler)
def runLabelling(self, f, frames,
Pid): # filename, frame numbers, transformation ids
Pid = Pid.tolist()
print frames
print Pid
cam_num = int(f[-5])
splitidx = string.index(f, 'split_')
split_num = int(f[splitidx + 6])
if split_num == 0:
split_num = 10
path, fname = os.path.split(f)
fname = fname[8:] # remove 'split_?'
args = parse_args(path, fname)
prefix = path + '/' + fname
params = args['params']
cam = params['cam'][cam_num - 1]
self.label_scale = np.array([[float(self.labelw) / cam['width']],
[float(self.labelh) / cam['height']]])
self.img_scale = np.array([[float(self.imwidth) / cam['width']],
[float(self.imheight) / cam['height']]])
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_mark2
gps_filename2 = args[gps_key2]
if not (gps_filename2 in self.gps_values2
): # if haven't read this gps file before, cache it in dict.
gps_reader2 = GPSReader(gps_filename2)
self.gps_values2[gps_filename2] = gps_reader2.getNumericData()
gps_data2 = self.gps_values2[gps_filename2]
gps_times2 = utc_from_gps_log_all(gps_data2)
# gps_mark1
gps_filename1 = args[gps_key1]
if not (gps_filename1 in self.gps_values1
): # if haven't read this gps file before, cache it in dict.
gps_reader1 = GPSReader(gps_filename1)
self.gps_values1[gps_filename1] = gps_reader1.getNumericData()
gps_data1 = self.gps_values1[gps_filename1]
tr1 = IMUTransforms(gps_data1)
gps_times1 = utc_from_gps_log_all(gps_data1)
prefix = gps_filename2[0:-postfix_len]
#lane_filename = prefix+'_multilane_points_done.npz'
lane_filename = prefix + '_multilane_points_planar_done.npz'
if not (lane_filename in self.lane_values):
self.lane_values[lane_filename] = np.load(lane_filename)
lanes = self.lane_values[
lane_filename] # these values are alread pre-computed and saved, now just read it from dictionary
start_frame = 0 #frames to skip #int(sys.argv[3]) if len(sys.argv) > 3 else 0
final_frame = -1 #int(sys.argv[4]) if len(sys.argv) > 4 else -1
return self.runBatch(f, gps_data1, gps_times1, gps_times2, frames,
start_frame, final_frame, lanes, tr1, Pid,
split_num, cam_num, params)
def get_transforms():
gps_reader = GPSReader(GPS_FILE)
gps_data = gps_reader.getNumericData()
imu_transforms = IMUTransforms(gps_data)
return imu_transforms
