Convert points in camera coordinate to lidar coordinate. Note: This function is for KITTI only. Args: points (np.ndarray, shape=[N, 3]): Points in camera coordinate. r_rect (np.ndarray, shape=[4, 4]): Matrix to project points in specific camera coordinat
(points, r_rect, velo2cam)
| 12 | |
| 13 | |
| 14 | def camera_to_lidar(points, r_rect, velo2cam): |
| 15 | """Convert points in camera coordinate to lidar coordinate. |
| 16 | |
| 17 | Note: |
| 18 | This function is for KITTI only. |
| 19 | |
| 20 | Args: |
| 21 | points (np.ndarray, shape=[N, 3]): Points in camera coordinate. |
| 22 | r_rect (np.ndarray, shape=[4, 4]): Matrix to project points in |
| 23 | specific camera coordinate (e.g. CAM2) to CAM0. |
| 24 | velo2cam (np.ndarray, shape=[4, 4]): Matrix to project points in |
| 25 | camera coordinate to lidar coordinate. |
| 26 | |
| 27 | Returns: |
| 28 | np.ndarray, shape=[N, 3]: Points in lidar coordinate. |
| 29 | """ |
| 30 | points_shape = list(points.shape[0:-1]) |
| 31 | if points.shape[-1] == 3: |
| 32 | points = np.concatenate([points, np.ones(points_shape + [1])], axis=-1) |
| 33 | lidar_points = points @ np.linalg.inv((r_rect @ velo2cam).T) |
| 34 | return lidar_points[..., :3] |
| 35 | |
| 36 | |
| 37 | def box_camera_to_lidar(data, r_rect, velo2cam): |
no outgoing calls
no test coverage detected