处理单个点云文件
(args)
| 79 | np.save(os.path.join(save_dir, f"{timestamp}.npy"), rt_matrix) |
| 80 | |
| 81 | def process_single_file(args): |
| 82 | """处理单个点云文件""" |
| 83 | fragment_path, pcd_file, point_step = args |
| 84 | timestamp = os.path.splitext(pcd_file)[0] |
| 85 | log_prefix = f"[{os.path.basename(os.path.dirname(fragment_path))}/{pcd_file}]" # 修复路径处理 |
| 86 | |
| 87 | try: |
| 88 | # 前车处理流程 |
| 89 | front_pcd = os.path.join(fragment_path, 'front', 'lidar', pcd_file) |
| 90 | fused_cloud = pcd_to_npy(front_pcd) |
| 91 | |
| 92 | # 后车处理 |
| 93 | back_pcd = os.path.join(fragment_path, 'back', 'lidar', pcd_file) |
| 94 | if os.path.exists(back_pcd): |
| 95 | t_sec = int(os.path.basename(fragment_path)) |
| 96 | t_full = float(timestamp) / 10 |
| 97 | |
| 98 | # 替换海象运算符 |
| 99 | gps_front_file = os.path.join(fragment_path, 'gps', 'front', f"{t_sec}.csv") |
| 100 | gps_back_file = os.path.join(fragment_path, 'gps', 'back', f"{t_sec}.csv") |
| 101 | |
| 102 | gpsA = read_gps_csv(gps_front_file, t_full) |
| 103 | gpsB = read_gps_csv(gps_back_file, t_full) |
| 104 | if gpsA and gpsB: |
| 105 | RT_init = CarB2CarA(gpsA, gpsB) |
| 106 | Rr, Tr, back_trans, *_ = pyicp( |
| 107 | back_pcd, front_pcd, |
| 108 | gt=RT_init, |
| 109 | point_step=point_step, |
| 110 | log_prefix=f"{log_prefix} [后车]" |
| 111 | ) |
| 112 | save_dir = os.path.join(fragment_path, 'front', 'back2front') |
| 113 | print(save_dir) |
| 114 | save_rt_matrix(Rr, Tr, save_dir, timestamp) |
| 115 | fused_cloud = np.vstack((fused_cloud, back_trans.T)) |
| 116 | |
| 117 | # 无人机处理 |
| 118 | top_pcd = os.path.join(fragment_path, 'top', 'lidar', pcd_file) |
| 119 | if os.path.exists(top_pcd): |
| 120 | imu_file = os.path.join(fragment_path, 'imu', 'top', f"{t_sec}.csv") |
| 121 | imu_data = read_imu_csv(imu_file, t_full) |
| 122 | if imu_data and gpsA: |
| 123 | RT_init = uav2car(gpsA[3], *imu_data) |
| 124 | Rr, Tr, uav_trans, *_ = pyicp( |
| 125 | top_pcd, front_pcd, |
| 126 | gt=RT_init, |
| 127 | point_step=point_step, |
| 128 | log_prefix=f"{log_prefix} [无人机]" |
| 129 | ) |
| 130 | save_dir = os.path.join(fragment_path, 'front', 'uav2car') |
| 131 | save_rt_matrix(Rr, Tr, save_dir, timestamp) |
| 132 | fused_cloud = np.vstack((fused_cloud, uav_trans.T)) |
| 133 | |
| 134 | # 保存融合点云 |
| 135 | save_fused_cloud(fused_cloud, fragment_path, pcd_file) |
| 136 | return True |
| 137 | except Exception as e: |
| 138 | log_error(f"{log_prefix} {str(e)}") |
nothing calls this directly
no test coverage detected