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 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 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()
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)