def plot_box(box, axis, view, colors, normalize=False, linewidth=1.0):
'''Plot boxes in the 3d figure'''
corners = view_points(box.corners(), view, normalize=normalize) #
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]],
[prev[2], corner[2]],
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]],
[corners.T[i][2], corners.T[i + 4][2]],
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]) #4x3
draw_rect(corners.T[4:], colors[1])
def get_lidar_points(lidar_token):
'''Get lidar point cloud in the frame of the ego vehicle'''
sd_record = lyftdata.get("sample_data", lidar_token)
sensor_modality = sd_record["sensor_modality"]
# Get aggregated point cloud in lidar frame.
sample_rec = lyftdata.get("sample", sd_record["sample_token"])
chan = sd_record["channel"]
ref_chan = "LIDAR_TOP"
pc, times = LidarPointCloud.from_file_multisweep(
lyftdata, sample_rec, chan, ref_chan, num_sweeps=1
)
# Compute transformation matrices for lidar point cloud
cs_record = lyftdata.get("calibrated_sensor", sd_record["calibrated_sensor_token"])
pose_record = lyftdata.get("ego_pose", sd_record["ego_pose_token"])
vehicle_from_sensor = np.eye(4)
vehicle_from_sensor[:3, :3] = Quaternion(cs_record["rotation"]).rotation_matrix
vehicle_from_sensor[:3, 3] = cs_record["translation"]
ego_yaw = Quaternion(pose_record["rotation"]).yaw_pitch_roll[0]
rot_vehicle_flat_from_vehicle = np.dot(
Quaternion(scalar=np.cos(ego_yaw / 2), vector=[0, 0, np.sin(ego_yaw / 2)]).rotation_matrix,
Quaternion(pose_record["rotation"]).inverse.rotation_matrix,
)
vehicle_flat_from_vehicle = np.eye(4)
vehicle_flat_from_vehicle[:3, :3] = rot_vehicle_flat_from_vehicle
points = view_points(
pc.points[:3, :], np.dot(vehicle_flat_from_vehicle, vehicle_from_sensor), normalize=False
)
return points
def plot_box(ax, box, lyftdata, view=np.eye(3), normalize=False, linewidth=1):
colors = [
'red', 'green', 'blue', 'yellow', 'cyan', 'magenta', 'orange', 'grey',
'black'
]
color_dict = {
cat_record['name']: c
for cat_record, c in zip(lyftdata.category, colors)
}
corners = view_points(box.corners(), view, normalize=normalize)[:2, :]
def draw_rect(selected_corners, color):
prev = selected_corners[-1]
for corner in selected_corners:
ax.plot([prev[0], corner[0]], [prev[1], corner[1]],
color=color,
linewidth=linewidth)
prev = corner
# Draw the sides
for i in range(4):
ax.plot(
[corners.T[i][0], corners.T[i + 4][0]],
[corners.T[i][1], corners.T[i + 4][1]],
color=color_dict[box.name],
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)
ax.plot(
[center_bottom[0], center_bottom_forward[0]],
[center_bottom[1], center_bottom_forward[1]],
color=colors[0],
linewidth=linewidth,
)