def render_sample_data(self,
sample_data_token: str,
with_anns: bool = True,
box_vis_level: BoxVisibility = 3,
axes_limit: float = 40,
ax: Axes = None) -> None:
"""
Render sample data onto axis.
:param sample_data_token: Sample_data token.
:param with_anns: Whether to draw annotations.
:param box_vis_level: If sample_data is an image, this sets required visibility for boxes.
:param axes_limit: Axes limit for lidar data (measured in meters).
:param ax: Axes onto which to render.
"""
sd_record = self.nusc.get('sample_data', sample_data_token)
sensor_modality = sd_record['sensor_modality']
data_path, boxes, camera_intrinsic = self.nusc.get_sample_data(
sample_data_token, box_vis_level=box_vis_level)
if sensor_modality == 'lidar':
data = PointCloud.from_file(data_path)
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 9))
points = view_points(data.points[:3, :],
np.eye(4),
normalize=False)
ax.scatter(points[0, :], points[1, :], c=points[2, :], s=1)
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=np.eye(4), colors=[c, c, c])
ax.set_xlim(-axes_limit, axes_limit)
ax.set_ylim(-axes_limit, axes_limit)
elif sensor_modality == 'camera':
data = Image.open(data_path)
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 16))
ax.imshow(data)
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax,
view=camera_intrinsic,
normalize=True,
colors=[c, c, c])
ax.set_xlim(0, data.size[0])
ax.set_ylim(data.size[1], 0)
else:
raise ValueError("RADAR rendering not implemented yet.")
ax.axis('off')
ax.set_title(sd_record['channel'])
ax.set_aspect('equal')
def getModel(PCs, boxes, offset=0, scale=1.0, normalize=False):
if len(PCs) == 0:
return PointCloud(np.ones((3, 0)))
points = np.ones((PCs[0].points.shape[0], 0))
for PC, box in zip(PCs, boxes):
cropped_PC = cropAndCenterPC(PC,
box,
offset=offset,
scale=scale,
normalize=normalize)
# try:
if cropped_PC.points.shape[1] > 0:
points = np.concatenate([points, cropped_PC.points], axis=1)
PC = PointCloud(points)
return PC
def getlabelPC(PC, box, offset=0, scale=1.0):
box_tmp = copy.deepcopy(box)
new_PC = PointCloud(PC.points.copy())
rot_mat = np.transpose(box_tmp.rotation_matrix)
trans = -box_tmp.center
# align data
new_PC.translate(trans)
box_tmp.translate(trans)
new_PC.rotate((rot_mat))
box_tmp.rotate(Quaternion(matrix=(rot_mat)))
box_tmp.wlh = box_tmp.wlh * scale
maxi = np.max(box_tmp.corners(), 1) + offset
mini = np.min(box_tmp.corners(), 1) - offset
x_filt_max = new_PC.points[0, :] < maxi[0]
x_filt_min = new_PC.points[0, :] > mini[0]
y_filt_max = new_PC.points[1, :] < maxi[1]
y_filt_min = new_PC.points[1, :] > mini[1]
z_filt_max = new_PC.points[2, :] < maxi[2]
z_filt_min = new_PC.points[2, :] > mini[2]
close = np.logical_and(x_filt_min, x_filt_max)
close = np.logical_and(close, y_filt_min)
close = np.logical_and(close, y_filt_max)
close = np.logical_and(close, z_filt_min)
close = np.logical_and(close, z_filt_max)
new_label = np.zeros(new_PC.points.shape[1])
new_label[close] = 1
return new_label
def cropPC(PC, box, offset=0, scale=1.0):
box_tmp = copy.deepcopy(box)
box_tmp.wlh = box_tmp.wlh * scale
maxi = np.max(box_tmp.corners(), 1) + offset
mini = np.min(box_tmp.corners(), 1) - offset
x_filt_max = PC.points[0, :] < maxi[0]
x_filt_min = PC.points[0, :] > mini[0]
y_filt_max = PC.points[1, :] < maxi[1]
y_filt_min = PC.points[1, :] > mini[1]
z_filt_max = PC.points[2, :] < maxi[2]
z_filt_min = PC.points[2, :] > mini[2]
close = np.logical_and(x_filt_min, x_filt_max)
close = np.logical_and(close, y_filt_min)
close = np.logical_and(close, y_filt_max)
close = np.logical_and(close, z_filt_min)
close = np.logical_and(close, z_filt_max)
new_PC = PointCloud(PC.points[:, close])
return new_PC
def getPCandBBfromPandas(self, box, calib):
center = [box["x"], box["y"] - box["height"] / 2, box["z"]]
size = [box["width"], box["length"], box["height"]]
orientation = Quaternion(axis=[0, 1, 0],
radians=box["rotation_y"]) * Quaternion(
axis=[1, 0, 0], radians=np.pi / 2)
BB = Box(center, size, orientation)
try:
# VELODYNE PointCloud
velodyne_path = os.path.join(self.KITTI_velo, box["scene"],
f'{box["frame"]:06}.bin')
PC = PointCloud(
np.fromfile(velodyne_path, dtype=np.float32).reshape(-1, 4).T)
PC.transform(calib)
except FileNotFoundError:
# in case the Point cloud is missing
# (0001/[000177-000180].bin)
PC = PointCloud(np.array([[0, 0, 0]]).T)
return PC, BB
def render_annotation(self,
anntoken: str,
margin: float = 10,
view: np.ndarray = np.eye(4),
box_vis_level: BoxVisibility = 3) -> None:
"""
Render selected annotation.
:param anntoken: Sample_annotation token.
:param margin: How many meters in each direction to include in LIDAR view.
:param view: LIDAR view point.
:param box_vis_level: If sample_data is an image, this sets required visibility for boxes.
"""
ann_record = self.nusc.get('sample_annotation', anntoken)
sample_record = self.nusc.get('sample', ann_record['sample_token'])
assert 'LIDAR_TOP' in sample_record['data'].keys(
), 'No LIDAR_TOP in data, cant render'
fig, axes = plt.subplots(1, 2, figsize=(18, 9))
# Figure out which camera the object is fully visible in (this may return nothing)
boxes, cam = [], []
cams = [key for key in sample_record['data'].keys() if 'CAM' in key]
for cam in cams:
_, boxes, _ = self.nusc.get_sample_data(
sample_record['data'][cam],
box_vis_level=box_vis_level,
selected_anntokens=[anntoken])
if len(boxes) > 0:
break # We found an image that matches. Let's abort.
assert len(
boxes
) > 0, "Could not find image where annotation if visible. Try using e.g. BoxVisibility.ANY."
assert len(
boxes) < 2, "Found multiple annotations. Something is wrong!"
cam = sample_record['data'][cam]
# Plot LIDAR view
lidar = sample_record['data']['LIDAR_TOP']
data_path, boxes, camera_intrinsic = self.nusc.get_sample_data(
lidar, selected_anntokens=[anntoken])
PointCloud.from_file(data_path).render_height(axes[0], view=view)
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(axes[0], view=view, colors=[c, c, c])
corners = view_points(boxes[0].corners(), view, False)[:2, :]
axes[0].set_xlim([
np.min(corners[0, :]) - margin,
np.max(corners[0, :]) + margin
])
axes[0].set_ylim([
np.min(corners[1, :]) - margin,
np.max(corners[1, :]) + margin
])
axes[0].axis('off')
axes[0].set_aspect('equal')
# Plot CAMERA view
data_path, boxes, camera_intrinsic = self.nusc.get_sample_data(
cam, selected_anntokens=[anntoken])
im = Image.open(data_path)
axes[1].imshow(im)
axes[1].set_title(self.nusc.get('sample_data', cam)['channel'])
axes[1].axis('off')
axes[1].set_aspect('equal')
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(axes[1],
view=camera_intrinsic,
normalize=True,
colors=[c, c, c])
def map_pointcloud_to_image(self, lidar_token: str, camera_token: str):
"""
Given a lidar and camera sample_data token, load point-cloud and map it to the image plane.
:param lidar_token: Lidar sample data token.
:param camera_token: Camera sample data token.
:return (pointcloud <np.float: 2, n)>, coloring <np.float: n>, image <Image>).
"""
cam = self.nusc.get('sample_data', camera_token)
top_lidar = self.nusc.get('sample_data', lidar_token)
pc = PointCloud.from_file(
osp.join(self.nusc.dataroot, top_lidar['filename']))
im = Image.open(osp.join(self.nusc.dataroot, cam['filename']))
# LIDAR points live in the lidar frame. So they need to be transformed via global to the image plane.
# First step: transform the point cloud to ego vehicle frame for the timestamp of the LIDAR sweep.
cs_record = self.nusc.get('calibrated_sensor',
top_lidar['calibrated_sensor_token'])
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix)
pc.translate(np.array(cs_record['translation']))
# Second step: transform to the global frame.
poserecord = self.nusc.get('ego_pose', top_lidar['ego_pose_token'])
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix)
pc.translate(np.array(poserecord['translation']))
# Third step: transform into the ego vehicle frame for the timestamp of the image.
poserecord = self.nusc.get('ego_pose', cam['ego_pose_token'])
pc.translate(-np.array(poserecord['translation']))
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix.T)
# Fourth step: transform into the camera.
cs_record = self.nusc.get('calibrated_sensor',
cam['calibrated_sensor_token'])
pc.translate(-np.array(cs_record['translation']))
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix.T)
# Fifth step: actually take a "picture" of the point cloud.
# Grab the depths (camera frame z axis points away from the camera).
depths = pc.points[2, :]
# Set the height to be the coloring.
coloring = pc.points[2, :]
# Take the actual picture (matrix multiplication with camera-matrix + renormalization).
points = view_points(pc.points[:3, :],
np.array(cs_record['camera_intrinsic']),
normalize=True)
# Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons.
mask = np.ones(depths.shape[0], dtype=bool)
mask = np.logical_and(mask, depths > 0)
mask = np.logical_and(mask, points[0, :] > 1)
mask = np.logical_and(mask, points[0, :] < im.size[0] - 1)
mask = np.logical_and(mask, points[1, :] > 1)
mask = np.logical_and(mask, points[1, :] < im.size[1] - 1)
points = points[:, mask]
coloring = coloring[mask]
return points, coloring, im