Spaces:
Sleeping
Sleeping
File size: 5,648 Bytes
a37f5d3 4490536 a37f5d3 4490536 a37f5d3 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 | 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)
|