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])