(pose_path, max_beams=None)
| 14 | |
| 15 | |
| 16 | def load_data(pose_path, max_beams=None): |
| 17 | # Read and parse the poses |
| 18 | timestamps = [] |
| 19 | poses_gt = [] |
| 20 | odoms = [] |
| 21 | scans = [] |
| 22 | params = {} |
| 23 | |
| 24 | try: |
| 25 | with open(pose_path, 'r') as f: |
| 26 | all_data = json.load(f) |
| 27 | |
| 28 | # downsample beams number according to the max_beams |
| 29 | if max_beams is not None: |
| 30 | downsample_factor = all_data['num_beams'] // max_beams |
| 31 | all_data['angle_res'] *= downsample_factor |
| 32 | all_data['num_beams'] = max_beams |
| 33 | all_data['angle_max'] = all_data['angle_min'] + all_data['angle_res'] * max_beams |
| 34 | |
| 35 | params.update({'num_beams': all_data['num_beams']}) |
| 36 | params.update({'angle_min': all_data['angle_min']}) |
| 37 | params.update({'angle_max': all_data['angle_max']}) |
| 38 | params.update({'angle_res': all_data['angle_res']}) |
| 39 | params.update({'max_range': all_data['max_range']}) |
| 40 | |
| 41 | near = 0.02 |
| 42 | far = np.floor(all_data['max_range']) |
| 43 | bound = np.array([near, far]) |
| 44 | # ray directions for all beams in the lidar coordinate, shape: (N, 2) |
| 45 | directions = get_ray_directions(all_data['angle_min'], all_data['angle_max'], |
| 46 | all_data['angle_res']) |
| 47 | |
| 48 | params.update({'near': near}) |
| 49 | params.update({'far': far}) |
| 50 | params.update({'bound': bound}) |
| 51 | params.update({'directions': directions}) |
| 52 | |
| 53 | for data in all_data['scans']: |
| 54 | timestamps.append(data['timestamp']) |
| 55 | |
| 56 | pose = data['pose_gt'] |
| 57 | poses_gt.append(pose) |
| 58 | |
| 59 | odom = data['odom_reading'] |
| 60 | odoms.append(odom) |
| 61 | |
| 62 | scan = np.array(data['range_reading']) |
| 63 | if max_beams is not None: |
| 64 | scan = scan[::downsample_factor][:max_beams] |
| 65 | scan[scan >= all_data['max_range']] = 0 |
| 66 | scans.append(scan) |
| 67 | |
| 68 | except FileNotFoundError: |
| 69 | print('Ground truth poses are not available.') |
| 70 | |
| 71 | return np.array(timestamps), np.array(poses_gt), np.array(odoms), np.array(scans), params |
| 72 | |
| 73 |
no test coverage detected