def getNextData(reader, LDRFrameMap): for idx in range(10): (success, img) = reader.getNextFrame() #img = cv2.flip(img,-1) if not success: reader.setFrame(3) ldr_frame = loadLDR(LDRFrameMap[reader.framenum]) return (success, img, ldr_frame)
def integrateClouds(ldr_map, imu_transforms, offset, num_steps, step, calibrationParameters): for t in range(num_steps): fnum = offset + t * step print fnum data = loadLDR(ldr_map[fnum]) # filter out the roof rack dist = np.sqrt(np.sum(data[:, 0:3]**2, axis=1)) # See LidarIntegrator.py to see how this is filtering data_filter_mask = (dist > 3) &\ (data[:, 3] > 40) &\ (np.abs(data[:, 1]) < 2.2) &\ (np.abs(data[:, 1]) > 1.2) &\ (data[:, 2] < -1.8) &\ (data[:, 2] > -2.5) data = data[data_filter_mask, :] # transform data into IMU frame at time t pts = data[:, 0:3].transpose() pts = np.vstack((pts, np.ones((1, pts.shape[1])))) T_from_l_to_i = calibrationParameters['lidar']['T_from_l_to_i'] pts = np.dot(T_from_l_to_i, pts) # transform data into imu_0 frame # Microseconds till end of the sweep times = data[:, 5] transform_points_in_sweep(pts, times, fnum, imu_transforms) pts = pts.transpose() # for exporting purposes pts_copy = np.array(pts[:, 0:3]) pts_copy = np.column_stack((pts_copy, np.array(data[:, 3]))) pts_copy = np.column_stack((pts_copy, fnum * np.ones( (pts.shape[0], 1)))) all_data.append(pts_copy)
def integrateClouds(ldr_map, imu_transforms, offset, num_steps, step, calibrationParameters): for t in range(num_steps): fnum = offset+t*step print fnum data = loadLDR(ldr_map[fnum]) # filter out the roof rack dist = np.sqrt(np.sum(data[:, 0:3] ** 2, axis=1)) # See LidarIntegrator.py to see how this is filtering data_filter_mask = (dist > 3) &\ (data[:, 3] > 40) &\ (np.abs(data[:, 1]) < 2.2) &\ (np.abs(data[:, 1]) > 1.2) &\ (data[:, 2] < -1.8) &\ (data[:, 2] > -2.5) data = data[data_filter_mask, :] # transform data into IMU frame at time t pts = data[:, 0:3].transpose() pts = np.vstack((pts, np.ones((1, pts.shape[1])))) T_from_l_to_i = calibrationParameters['lidar']['T_from_l_to_i'] pts = np.dot(T_from_l_to_i, pts) # transform data into imu_0 frame # Microseconds till end of the sweep times = data[:, 5] transform_points_in_sweep(pts, times, fnum, imu_transforms) pts = pts.transpose() # for exporting purposes pts_copy = np.array(pts[:, 0:3]) pts_copy = np.column_stack((pts_copy, np.array(data[:, 3]))) pts_copy = np.column_stack((pts_copy, fnum*np.ones((pts.shape[0], 1)))) all_data.append(pts_copy)
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 dist = np.sqrt(np.sum(data[:, 0:3]**2, axis=1)) if LANE_FILTER: data_filter_mask = (dist > 3) & \ (data[:, 3] > 40) & \ (np.abs(data[:, 1]) < 2.2) & \ (np.abs(data[:, 1]) > 1.2) & \ (data[:, 2] < -1.8) & \ (data[:, 2] > -2.5) else: data_filter_mask = (dist > 3) & \
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 dist = np.sqrt(np.sum(data[:, 0:3] ** 2, axis=1)) if LANE_FILTER: data_filter_mask = (dist > 3) & \ (data[:, 3] > 40) & \ (np.abs(data[:, 1]) < 2.2) & \ (np.abs(data[:, 1]) > 1.2) & \ (data[:, 2] < -1.8) & \ (data[:, 2] > -2.5) else: data_filter_mask = (dist > 3) & \
ldr_interp[k::ni, 0:4] = ((ni - k) * ldr0[inds0, 0:4] + (k + 1.0) * ldr1[inds1, 0:4]) / (ni + 1.0) # Average the time as well ldr_interp[k::ni, 5] = ((ni + - k) * ldr0[inds0, 5] + (k + 1.0) * ldr1[inds1, 5]) / (ni + 1.0) # Concatenate the laser numbers ldr_interp[k::ni, 4] = ldr0[inds0, 4] * 100 + ldr1[inds1, 4] return ldr_interp if __name__ == '__main__': infile = sys.argv[1] outfile = sys.argv[2] ni = int(sys.argv[3]) ldr_data = loadLDR(infile) laser_nums = ldr_data[:, 4] # Laser num goes from 0 to 31 print np.min(laser_nums), np.max(laser_nums) laser_data = list() laser_nbrs = list() for laser_num in range(32): ld = ldr_data[laser_nums == laser_num, :] nbrs = NearestNeighbors(n_neighbors=1, algorithm='ball_tree').fit(ld[:, 0:3]) laser_data.append(ld) laser_nbrs.append(nbrs) interp_data = list() # Refer to HDL-32E manual for laser ordering