def get_radar_pointcloud(self, channel='RADAR_FRONT'):
"""
Extracting radar detection pointcloud with velocities for current timestep in current scene for specified radar channel.
:param channel: Radar channel selection. Front radar as default
:return (Point cloud [Position(x,y,z) X n_points], Point cloud [Velocity(x,y,z) X n_points])
"""
# Check for correct radar channel selection
assert channel in self.radar_sensor_channels, " [!] Radar channel \"{}\" not found.".format(
channel)
# Select sensor data record
sample_data_token = self.sample['data'][channel]
sd_record = self.nusc.get('sample_data', sample_data_token)
lidar_token = self.sample['data']['LIDAR_TOP']
# The point cloud is transformed to the lidar frame for visualization purposes.
ref_chan = 'LIDAR_TOP'
pc, times = RadarPointCloud.from_file_multisweep(self.nusc,
self.sample,
channel,
ref_chan,
nsweeps=1)
# Transform radar velocities (x is front, y is left), as these are not transformed when loading the point
# cloud.
radar_cs_record = self.nusc.get('calibrated_sensor',
sd_record['calibrated_sensor_token'])
lidar_sd_record = self.nusc.get('sample_data', lidar_token)
lidar_cs_record = self.nusc.get(
'calibrated_sensor', lidar_sd_record['calibrated_sensor_token'])
velocities = pc.points[8:10, :] # Compensated velocity
velocities = np.vstack((velocities, np.zeros(pc.points.shape[1])))
velocities = np.dot(
Quaternion(radar_cs_record['rotation']).rotation_matrix,
velocities)
velocities = np.dot(
Quaternion(lidar_cs_record['rotation']).rotation_matrix.T,
velocities)
velocities[2, :] = np.zeros(pc.points.shape[1])
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
points_vel = view_points(pc.points[:3, :] + velocities,
np.eye(4),
normalize=False)
print(" [*] Radar point cloud extracted from channel \"" + channel +
"\". Shape: " + str(points.shape))
return points, points_vel
def render_cv2(self, im, view=np.eye(3), normalize=False,
colors=((0, 0, 255), (255, 0, 0), (155, 155, 155)), linewidth=2):
"""
Renders box using opencv2.
:param im: <np.array: width, height, 3>. Image array. Channels are in BGR order.
:param view: <np.array: 3, 3>. Define a projection in needed (e.g. for drawing projection in an image).
:param normalize: <bool>. Whether to normalize the remaining coordinate.
:param colors: ((R, G, B), (R, G, B), (R, G, B)). Colors for front, side & rear.
:param linewidth: <float>. Linewidth for plot.
:return:
"""
corners = view_points(self.corners(), view, normalize=normalize)[:2, :]
def draw_rect(selected_corners, color):
prev = selected_corners[-1]
for corner in selected_corners:
cv2.line(im, (int(prev[0]), int(prev[1])), (int(corner[0]), int(corner[1])), color, linewidth)
prev = corner
# Draw the sides
for i in range(4):
cv2.line(im, (int(corners.T[i][0]), int(corners.T[i][1])),
(int(corners.T[i + 4][0]), int(corners.T[i + 4][1])), colors[2][::-1], linewidth)
# Draw front (first 4 corners) and rear (last 4 corners) rectangles(3d)/lines(2d)
draw_rect(corners.T[:4], colors[0][::-1])
draw_rect(corners.T[4:], colors[1][::-1])
# Draw line indicating the front
center_bottom_forward = np.mean(corners.T[2:4], axis=0)
center_bottom = np.mean(corners.T[[2, 3, 7, 6]], axis=0)
cv2.line(im, (int(center_bottom[0]), int(center_bottom[1])),
(int(center_bottom_forward[0]), int(center_bottom_forward[1])), colors[0][::-1], linewidth)
def render(self, axis, view=np.eye(3), normalize=False, colors=('b', 'r', 'k'), linewidth=2):
"""
Renders the box in the provided Matplotlib axis.
:param axis: <matplotlib.pyplot.axis>. Axis onto which the box should be drawn.
:param view: <np.array: 3, 3>. Define a projection in needed (e.g. for drawing projection in an image).
:param normalize: <bool>. Whether to normalize the remaining coordinate.
:param colors: (<Matplotlib.colors>: 3). Valid Matplotlib colors (<str> or normalized RGB tuple) for front,
back and sides.
:param linewidth: <float>. Width in pixel of the box sides.
"""
corners = view_points(self.corners(), view, normalize=normalize)[:2, :]
def draw_rect(selected_corners, color):
prev = selected_corners[-1]
for corner in selected_corners:
axis.plot([prev[0], corner[0]], [prev[1], corner[1]], color=color, linewidth=linewidth)
prev = corner
# Draw the sides
for i in range(4):
axis.plot([corners.T[i][0], corners.T[i + 4][0]], [corners.T[i][1], corners.T[i + 4][1]], color=colors[2],
linewidth=linewidth)
# Draw front (first 4 corners) and rear (last 4 corners) rectangles(3d)/lines(2d)
draw_rect(corners.T[:4], colors[0])
draw_rect(corners.T[4:], colors[1])
# Draw line indicating the front
center_bottom_forward = np.mean(corners.T[2:4], axis=0)
center_bottom = np.mean(corners.T[[2, 3, 7, 6]], axis=0)
axis.plot([center_bottom[0], center_bottom_forward[0]],
[center_bottom[1], center_bottom_forward[1]],
color=colors[0], linewidth=linewidth)
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
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 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 box_3d_to_2d(boxes, intrinsic):
"""
Get x and y of the 8 corners of a 3D box after mapping it to the image
viewpoint
"""
corners_list = []
for box in boxes:
corners_3d = box.corners()
corners_img = view_points(corners_3d, intrinsic, normalize=True)[:2, :]
corners_list.append(corners_img)
return corners_list
def _render_helper(self, color_channel, ax, view, x_lim, y_lim, marker_size):
"""
Helper function for rendering.
:param color_channel: <int>.
:param ax: <matplotlib.axes.Axes>. Axes on which to render the points.
:param view: <np.float: n, n>. Defines an arbitrary projection (n <= 4).
:param x_lim: (min <float>, max <float>).
:param y_lim: (min <float>, max <float>).
:param marker_size: <float>. Marker size.
:return: <None>.
"""
points = view_points(self.points[:3, :], view, normalize=False)
ax.scatter(points[0, :], points[1, :], c=self.points[color_channel, :], s=marker_size)
ax.set_xlim(x_lim)
ax.set_ylim(y_lim)
def _render_helper(self, color_channel: int, ax: Axes, view: np.ndarray,
x_lim: Tuple, y_lim: Tuple, marker_size: float) -> None:
"""
Helper function for rendering.
:param color_channel: Point channel to use as color.
:param ax: Axes on which to render the points.
:param view: <np.float: n, n>. Defines an arbitrary projection (n <= 4).
:param x_lim: (min <float>, max <float>).
:param y_lim: (min <float>, max <float>).
:param marker_size: Marker size.
"""
points = view_points(self.points[:3, :], view, normalize=False)
ax.scatter(points[0, :],
points[1, :],
c=self.points[color_channel, :],
s=marker_size)
ax.set_xlim(x_lim[0], x_lim[1])
ax.set_ylim(y_lim[0], y_lim[1])
def get_lidar_pointcloud(self):
"""
Extracting lidar pointcloud for current timestep in current scene
:return Point cloud [Position(x,y,z) X n_points]
"""
# Select sensor data record
channel = 'LIDAR_TOP'
sample_data_token = self.sample['data'][channel]
sd_record = self.nusc.get('sample_data', sample_data_token)
# Get aggregated point cloud in lidar frame.
chan = sd_record['channel']
pc, times = LidarPointCloud.from_file_multisweep(self.nusc,
self.sample,
chan,
channel,
nsweeps=1)
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
print(" [*] Lidar point cloud extracted. " + str(points.shape))
return points
def to_coco_bbox(self, view, imsize, wlh_factor: float = 1.0):
"""
Convert the 3d box to the 2D bounding box in COCO format (x,y,h,w)
:param view
:param imsize: Image size in pixels
:param wlh_factor: <float>. Multiply w, l, h by a factor to inflate or deflate the box.
:return: <np.float: 2, 4>. Corners of the 2D box
"""
neighbor_map = {
0: [1, 3, 4],
1: [0, 2, 5],
2: [1, 3, 6],
3: [0, 2, 7],
4: [0, 5, 7],
5: [1, 4, 6],
6: [2, 5, 7],
7: [3, 4, 6]
}
border_lines = [[(0, imsize[1]), (0, 0)],
[(imsize[0], 0), (imsize[0], imsize[1])],
[(imsize[0], imsize[1]), (0, imsize[1])],
[(0, 0), (imsize[0], 0)]]
# Map corners to the 2D plane of the camera perspective
corners_3d = self.corners(
wlh_factor=wlh_factor) # x:forward, y:left, z:up
corners_2d = view_points(corners_3d, view, normalize=True)[:2, :]
# Find corners that are outside image boundaries
invisible = np.logical_or(corners_2d[0, :] < 0,
corners_2d[0, :] > imsize[0])
invisible = np.logical_or(invisible, corners_2d[1, :] > imsize[1])
invisible = np.logical_or(invisible, corners_2d[1, :] < 0)
ind_invisible = [i for i, x in enumerate(invisible) if x]
corners_2d_visible = np.delete(corners_2d, ind_invisible, 1)
# Find intersections with boundary lines
for ind in ind_invisible:
# intersections = []
invis_point = (corners_2d[0, ind], corners_2d[1, ind])
for i in neighbor_map[ind]:
if i in ind_invisible:
# Both corners outside image boundaries, ignore them
continue
nbr_point = (corners_2d[0, i], corners_2d[1, i])
line = LineString([invis_point, nbr_point])
for borderline in border_lines:
intsc = line.intersection(LineString(borderline))
if not intsc.is_empty:
corners_2d_visible = np.append(
corners_2d_visible,
np.asarray([[intsc.x], [intsc.y]]), 1)
break
# Construct a 2D box covering the whole object
x_min, y_min = np.amin(corners_2d_visible, 1)
x_max, y_max = np.amax(corners_2d_visible, 1)
# Get the box corners
corners_2d = np.array([[x_max, x_max, x_min, x_min],
[y_max, y_min, y_min, y_max]])
# Convert to the MS COCO bbox format
# bbox = [corners_2d[0,3], corners_2d[1,3],
# corners_2d[0,0]-corners_2d[0,3],corners_2d[1,1]-corners_2d[1,0]]
bbox = [x_min, y_min, abs(x_max - x_min), abs(y_max - y_min)]
return bbox
def pointcloud_color_from_image(nusc, pointsensor_token: str, camera_token: str) -> Tuple[np.array, np.array]:
"""
Given a point sensor (lidar/radar) token and camera sample_data token, load point-cloud and map it to the image
plane, then retrieve the colors of the closest image pixels.
:param pointsensor_token: Lidar/radar sample_data token.
:param camera_token: Camera sample data token.
:return (coloring <np.float: 3, n>, mask <np.bool: m>). Returns the colors for n points that reproject into the
image out of m total points. The mask indicates which points are selected.
"""
cam = nusc.get('sample_data', camera_token)
pointsensor = nusc.get('sample_data', pointsensor_token)
print(nusc.dataroot)
print(pointsensor['filename'])
pc = PointCloud.from_file(osp.join(nusc.dataroot, pointsensor['filename']))
im = Image.open(osp.join(nusc.dataroot, cam['filename']))
# Points live in the point sensor frame. So they need to be transformed via global to the image plane.
# First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = nusc.get('calibrated_sensor', pointsensor['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 = nusc.get('ego_pose', pointsensor['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 = 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 = 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, :]
# 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]
# Pick the colors of the points
im_data = np.array(im)
coloring = np.zeros(points.shape)
for i, p in enumerate(points.transpose()):
point = p[:2].round().astype(np.int32)
coloring[:, i] = im_data[point[1], point[0], :]
return coloring, mask
point_cloud = PointCloud.from_file(os.path.join(nusc.dataroot, pointsensor['filename']))
# First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = nusc.get('calibrated_sensor', pointsensor['calibrated_sensor_token'])
point_cloud.rotate(Quaternion(cs_record['rotation']).rotation_matrix)
point_cloud.translate(np.array(cs_record['translation']))
# Forth step: transform the point-cloud to camera frame
cs_record = nusc.get('calibrated_sensor', cam['calibrated_sensor_token'])
point_cloud.translate(-np.array(cs_record['translation']))
point_cloud.rotate(Quaternion(cs_record['rotation']).rotation_matrix.T)
depths = point_cloud.points[2, :]
distances = []
for i in range(len(point_cloud.points[0, :])):
point = point_cloud.points[:, i]
distance = sqrt((point[0] ** 2) + (point[1] ** 2) + (point[2] ** 2))
distances.append(distance)
points = view_points(point_cloud.points[:3, :], np.array(cs_record['camera_intrinsic']), normalize=True)
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, :] < 1600 - 1)
mask = np.logical_and(mask, points[1, :] > 1)
mask = np.logical_and(mask, points[1, :] < 900 - 1)
points = points[:, mask]
distances_numpy = np.asarray(distances)
distances_numpy = distances_numpy[mask]
max_distance = max(distances_numpy)
norm = mpl.colors.Normalize(vmin=0, vmax=320)
cmap = cm.jet
m = cm.ScalarMappable(norm=norm, cmap=cmap)
for i in range(len(points[0, :])):
posX = int(points[0, i])
