Spaces:
Sleeping
Sleeping
| import numpy as np | |
| import os | |
| from utils.utils import read_calib_file #, compute_os1_angles, elevation_to_beam_id | |
| INLIER_BIT = 1 << 8 | |
| INSTANCE_SHIFT = 16 | |
| beam_altitude_angles_deg = np.array([ | |
| 20.97, 18.51, 15.97, 13.37, 12.04, 10.70, 9.35, 8.70, | |
| 7.99, 7.34, 6.62, 5.96, 5.23, 4.55, 3.84, 3.51, | |
| 3.17, 2.82, 2.46, 2.11, 1.76, 1.43, 1.05, 0.70, | |
| 0.36, 0.03, -0.34, -0.70, -1.04, -1.37, -1.76, -2.10, | |
| -2.43, -2.77, -3.15, -3.48, -3.84, -4.18, -4.54, -4.88, | |
| -5.24, -5.55, -5.93, -6.29, -6.63, -6.94, -7.32, -7.67, | |
| -8.00, -8.33, -9.04, -9.71, -10.42, -11.06, -11.77, -12.41, | |
| -13.12, -13.74, -15.06, -16.36, -17.64, -18.91, -20.16, -21.38 | |
| ]) | |
| class PointCloud: | |
| def __init__(self, file_path, calib_path): | |
| self.points = self.load_pointcloud(file_path) | |
| ground_labels = np.fromfile(f"{file_path.split('/ouster')[0]}/ouster_ground_labels/{file_path.split('/')[-1].split('.bin')[0]}.label", dtype=np.uint32) | |
| # points = self.load_pointcloud(file_path) | |
| # self.points = points[self.ground_labels==0,:] | |
| self.ground_semantic = ground_labels & 0xFF | |
| self.ground_inlier = (ground_labels & INLIER_BIT) != 0 | |
| calibration = read_calib_file(calib_path) | |
| self.P2, self.R0, self.Tr4 = self.get_matrices(calibration) | |
| self.pixel_shift_by_row = [12, 12, 12, 12, 12, 12, 12, -4, | |
| 12, -4, 12, -4, 12, -4, 12, 4, | |
| -4, -12, 12, 4, -4, -12, 12, 4, | |
| -4, -12, 12, 4, -4, -12, 12, 4, | |
| -4, -12, 12, 4, -4, -12, 12, 4, | |
| -4, -12, 12, 4, -4, -12, 12, 4, | |
| -4, -12, 4, -12, 4, -12, 4, -12, | |
| 4, -12, -12, -12, -12, -12, -12, -12] | |
| def load_pointcloud(self, file_path): | |
| """ | |
| Load point cloud from various formats including .bin files | |
| Assumes each point has 4 values (x, y, z, reflectivity) | |
| Args: | |
| file_path (str): Path to point cloud file | |
| Returns: | |
| np.array: Nx4 array of 3D points with | |
| """ | |
| file_ext = os.path.splitext(file_path)[1].lower() | |
| if not file_ext == '.bin': | |
| raise ValueError(f"Given file is not binary format: {file_path}") | |
| points = np.fromfile(file_path, dtype=np.float32) | |
| if len(points) % 4 == 0: | |
| points_array = points.reshape(-1, 4) | |
| return points_array | |
| def get_matrices(self,calib): | |
| # P2 (3x4) | |
| P2 = calib['P2'].reshape(3,4) | |
| # R0_rect (3x3) or identity | |
| R0 = calib.get('R0_rect', np.array([1,0,0,0,1,0,0,0,1])).reshape(3,3) | |
| # Tr_ouster_to_cam (3x4) | |
| Tr = calib['Tr_ouster_to_cam'].reshape(3,4) | |
| # Make 4x4 homogeneous | |
| Tr4 = np.vstack((Tr, [0,0,0,1])) | |
| return P2, R0, Tr4 | |
| def points_ouster_to_cam(self,): | |
| """ | |
| pts_ouster: (N,3) numpy array in LiDAR frame. | |
| Tr4: 4x4 homogeneous transform from ouster -> camera. | |
| Returns pts_cam (N,3) in camera coordinates. | |
| """ | |
| n = self.points.shape[0] | |
| pts_xyz = self.points[:, :3] | |
| pts_h = np.column_stack((pts_xyz, np.ones((n)))) # (N,4) | |
| pts_cam_h = (self.Tr4 @ pts_h.T).T[:,:3] # (N,4) | |
| valid = pts_cam_h[:,2] > 1e-6 | |
| distances = np.linalg.norm(pts_cam_h[valid,:], axis=1) | |
| return pts_cam_h[valid,:], np.linalg.norm(pts_cam_h, axis=1), self.points[:,3], pts_cam_h, valid | |
| def select_points_ouster_to_cam(self, points): | |
| """ | |
| pts_ouster: (N,3) numpy array in LiDAR frame. | |
| Tr4: 4x4 homogeneous transform from ouster -> camera. | |
| Returns pts_cam (N,3) in camera coordinates. | |
| """ | |
| n = points.shape[0] | |
| pts_h = np.column_stack((points[:,:3], np.ones((n)))) # (N,4) | |
| pts_cam_h = (self.Tr4 @ pts_h.T).T[:,:3] # (N,4) | |
| valid = pts_cam_h[:,2] > 1e-6 | |
| # valid = np.ones((pts_cam_h.shape[0])) | |
| distances = np.linalg.norm(pts_cam_h[valid,:], axis=1) | |
| return pts_cam_h[valid,:], distances, points[valid,3] | |
| def destagger(self): | |
| pixel_shift_by_row = np.array(self.pixel_shift_by_row) | |
| H = 64 | |
| W = 1024 | |
| # pc_img = points.reshape(W, H, 4).transpose(1, 0, 2) | |
| # # shape: (64, 1024, 4) | |
| # rows = np.arange(H)[:, None] | |
| # cols = np.arange(W)[None, :] | |
| # pc_destaggered = pc_img[ | |
| # rows, | |
| # (cols - pixel_shift_by_row[:, None]) % W, | |
| # : | |
| # ] | |
| # return pc_destaggered.reshape(-1, 4) | |
| # --- reshape --- | |
| pc_img = self.points.reshape(W, H, 4).transpose(1, 0, 2) # (64, 1024, 4) | |
| lbl_img = self.ground_semantic.reshape(W, H).T # (64, 1024) | |
| inlier_img = self.ground_inlier.reshape(W, H).T | |
| rows = np.arange(H)[:, None] | |
| cols = np.arange(W)[None, :] | |
| # --- destagger (IDENTICAL indexing) --- | |
| pc_destaggered = pc_img[ | |
| rows, | |
| (cols - pixel_shift_by_row[:, None]) % W, | |
| : | |
| ] | |
| lbl_destaggered = lbl_img[ | |
| rows, | |
| (cols - pixel_shift_by_row[:, None]) % W | |
| ] | |
| inlier_destaggered = inlier_img[ | |
| rows, | |
| (cols - pixel_shift_by_row[:, None]) % W | |
| ] | |
| return pc_destaggered.reshape(-1, 4), lbl_destaggered.reshape(-1), inlier_destaggered.reshape(-1) | |