def _get_radar_data(self, sample_rec: dict, sample_data: dict, nsweeps: int) -> RadarPointCloud: """ Returns Radar point cloud in Vehicle Coordinates :param sample_rec: sample record dictionary from nuscenes :param sample_data: sample data dictionary from nuscenes :param nsweeps: number of sweeps to return for this radar :return pc: RadarPointCloud containing this samnple and all sweeps """ radar_path = os.path.join(self.nusc_path, sample_data['filename']) cs_record = self.nusc.get('calibrated_sensor', sample_data['calibrated_sensor_token']) if nsweeps > 1: ## Returns in vehicle coordinates pc, _ = RadarPointCloud.from_file_multisweep( self.nusc, sample_rec, sample_data['channel'], sample_data['channel'], nsweeps=nsweeps, min_distance=self.radar_min_distance) else: ## Returns in sensor coordinates pc = RadarPointCloud.from_file(radar_path) ## Sensor to vehicle rot_matrix = Quaternion(cs_record['rotation']).rotation_matrix pc.rotate(rot_matrix) pc.translate(np.array(cs_record['translation'])) return pc
def get_data(sensor_data): sd_record = nusc.get('sample_data', sensor_data) sensor_modality = sd_record['sensor_modality'] nsweeps = 1 sample_rec = nusc.get('sample', sd_record['sample_token']) ref_chan = 'LIDAR_TOP' chan = sd_record['channel'] if sensor_modality == 'lidar': pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps) points = pc.points return points # (4, 24962) max 30000 elif sensor_modality == 'radar': pc, times = RadarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps) points = pc.points return points # (18, -1) # max 100 elif sensor_modality == 'camera': data_path, boxes, camera_intrinsic = nusc.get_sample_data( sensor_data) #, box_vis_level=box_vis_level) data = Image.open(data_path) img = np.array(data) return img # (900, 1600, 3)
def test_load_pointclouds(self): """ Loads up lidar and radar pointclouds. """ assert 'NUSCENES' in os.environ, 'Set NUSCENES env. variable to enable tests.' dataroot = os.environ['NUSCENES'] nusc = NuScenes(version='v1.0-mini', dataroot=dataroot, verbose=False) sample_rec = nusc.sample[0] lidar_name = nusc.get('sample_data', sample_rec['data']['LIDAR_TOP'])['filename'] radar_name = nusc.get('sample_data', sample_rec['data']['RADAR_FRONT'])['filename'] lidar_path = os.path.join(dataroot, lidar_name) radar_path = os.path.join(dataroot, radar_name) pc1 = LidarPointCloud.from_file(lidar_path) pc2 = RadarPointCloud.from_file(radar_path) pc3, _ = LidarPointCloud.from_file_multisweep(nusc, sample_rec, 'LIDAR_TOP', 'LIDAR_TOP', nsweeps=2) pc4, _ = RadarPointCloud.from_file_multisweep(nusc, sample_rec, 'RADAR_FRONT', 'RADAR_FRONT', nsweeps=2) # Check for valid dimensions. assert pc1.points.shape[0] == pc3.points.shape[ 0] == 4, 'Error: Invalid dimension for lidar pointcloud!' assert pc2.points.shape[0] == pc4.points.shape[ 0] == 18, 'Error: Invalid dimension for radar pointcloud!' assert pc1.points.dtype == pc3.points.dtype, 'Error: Invalid dtype for lidar pointcloud!' assert pc2.points.dtype == pc4.points.dtype, 'Error: Invalid dtype for radar pointcloud!'
def get_sensor_sample_data(nusc, sample, sensor_channel, dtype=np.float32, size=None): """ This function takes the token of a sample and a sensor sensor_channel and returns the according data :param sample: the nuscenes sample dict :param sensor_channel: the target sensor channel of the given sample to load the data from :param dtype: the target numpy type :param size: for resizing the image Radar Format: - Shape: 19 x n - Semantics: [0]: x (1) [1]: y (2) [2]: z (3) [3]: dyn_prop (4) [4]: id (5) [5]: rcs (6) [6]: vx (7) [7]: vy (8) [8]: vx_comp (9) [9]: vy_comp (10) [10]: is_quality_valid (11) [11]: ambig_state (12) [12]: x_rms (13) [13]: y_rms (14) [14]: invalid_state (15) [15]: pdh0 (16) [16]: vx_rms (17) [17]: vy_rms (18) [18]: distance (19) """ # Get filepath sd_rec = nusc.get('sample_data', sample['data'][sensor_channel]) file_name = osp.join(nusc.dataroot, sd_rec['filename']) # Check conditions if not osp.exists(file_name): raise FileNotFoundError("nuscenes data must be located in %s" % file_name) # Read the data if "RADAR" in sensor_channel: pcs, times = RadarPointCloud.from_file_multisweep(nusc, sample, sensor_channel, \ sensor_channel, nsweeps=required_sweep_count, min_distance=0.0) # Load radar points data = pcs.points.astype(dtype) else: raise Exception("\"%s\" is not supported" % sensor_channel) return data
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_sample_data(self, sample_data_token: str, with_anns: bool = True, box_vis_level: BoxVisibility = BoxVisibility.ANY, axes_limit: float = 40, ax: Axes = None, nsweeps: int = 1) -> 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 and radar (measured in meters). :param ax: Axes onto which to render. :param nsweeps: Number of sweeps for lidar and radar. """ # Get sensor modality. sd_record = self.nusc.get('sample_data', sample_data_token) sensor_modality = sd_record['sensor_modality'] if sensor_modality == 'lidar': # Get boxes in lidar frame. _, boxes, _ = self.nusc.get_sample_data(sample_data_token, box_vis_level=box_vis_level) # Get aggregated point cloud in lidar frame. sample_rec = self.nusc.get('sample', sd_record['sample_token']) chan = sd_record['channel'] ref_chan = 'LIDAR_TOP' pc, times = LidarPointCloud.from_file_multisweep(self.nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps) # Init axes. if ax is None: _, ax = plt.subplots(1, 1, figsize=(9, 9)) # Show point cloud. points = view_points(pc.points[:3, :], np.eye(4), normalize=False) dists = np.sqrt(np.sum(pc.points[:2, :] ** 2, axis=0)) colors = np.minimum(1, dists/axes_limit/np.sqrt(2)) ax.scatter(points[0, :], points[1, :], c=colors, s=0.2) # Show ego vehicle. ax.plot(0, 0, 'x', color='black') # Show boxes. 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)) # Limit visible range. ax.set_xlim(-axes_limit, axes_limit) ax.set_ylim(-axes_limit, axes_limit) elif sensor_modality == 'radar': # Get boxes in lidar frame. sample_rec = self.nusc.get('sample', sd_record['sample_token']) lidar_token = sample_rec['data']['LIDAR_TOP'] _, boxes, _ = self.nusc.get_sample_data(lidar_token, box_vis_level=box_vis_level) # Get aggregated point cloud in lidar frame. # The point cloud is transformed to the lidar frame for visualization purposes. chan = sd_record['channel'] ref_chan = 'LIDAR_TOP' pc, times = RadarPointCloud.from_file_multisweep(self.nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps) # 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]) # Init axes. if ax is None: _, ax = plt.subplots(1, 1, figsize=(9, 9)) # Show point cloud. points = view_points(pc.points[:3, :], np.eye(4), normalize=False) dists = np.sqrt(np.sum(pc.points[:2, :] ** 2, axis=0)) colors = np.minimum(1, dists / axes_limit / np.sqrt(2)) sc = ax.scatter(points[0, :], points[1, :], c=colors, s=3) # Show velocities. points_vel = view_points(pc.points[:3, :] + velocities, np.eye(4), normalize=False) max_delta = 10 deltas_vel = points_vel - points deltas_vel = 3 * deltas_vel # Arbitrary scaling deltas_vel = np.clip(deltas_vel, -max_delta, max_delta) # Arbitrary clipping colors_rgba = sc.to_rgba(colors) for i in range(points.shape[1]): ax.arrow(points[0, i], points[1, i], deltas_vel[0, i], deltas_vel[1, i], color=colors_rgba[i]) # Show ego vehicle. ax.plot(0, 0, 'x', color='black') # Show boxes. 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)) # Limit visible range. ax.set_xlim(-axes_limit, axes_limit) ax.set_ylim(-axes_limit, axes_limit) elif sensor_modality == 'camera': # Load boxes and image. data_path, boxes, camera_intrinsic = self.nusc.get_sample_data(sample_data_token, box_vis_level=box_vis_level) data = Image.open(data_path) # Init axes. if ax is None: _, ax = plt.subplots(1, 1, figsize=(9, 16)) # Show image. ax.imshow(data) # Show boxes. 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)) # Limit visible range. ax.set_xlim(0, data.size[0]) ax.set_ylim(data.size[1], 0) else: raise ValueError("Error: Unknown sensor modality!") ax.axis('off') ax.set_title(sd_record['channel']) ax.set_aspect('equal')
def create_annotations(self, sample_token, sensor_channels): """ Create annotations for the the given sample token. 1 bounding box vector contains: :param sample_token: the sample_token to get the annotation for :param sensor_channels: list of channels for cropping the labels, e.g. ['CAM_FRONT', 'RADAR_FRONT'] This works only for CAMERA atm :returns: annotations dictionary: { 'labels': [] # <list of n int> 'bboxes': [] # <list of n x 4 float> [xmin, ymin, xmax, ymax] 'distances': [] # <list of n float> Center of box given as x, y, z. 'visibilities': [] # <list of n float> Visibility of annotated object } """ if any([s for s in sensor_channels if 'RADAR' in s]): print("[WARNING] Cropping to RADAR is not supported atm") sensor_channels = [ c for c in sensor_channels if 'CAM' in sensor_channels ] sample = self.nusc.get('sample', sample_token) annotations_count = 0 annotations = { 'labels': [], # <list of n int> 'bboxes': [], # <list of n x 4 float> [xmin, ymin, xmax, ymax] 'distances': [], # <list of n float> Center of box given as x, y, z. 'visibilities': [], 'num_radar_pts': [ ] #<list of n int> number of radar points that cover that annotation } # Camera parameters for selected_sensor_channel in sensor_channels: sd_rec = self.nusc.get('sample_data', sample['data'][selected_sensor_channel]) # Create Boxes: _, boxes, camera_intrinsic = self.nusc.get_sample_data( sd_rec['token'], box_vis_level=BoxVisibility.ANY) imsize_src = (sd_rec['width'], sd_rec['height'] ) # nuscenes has (width, height) convention bbox_resize = [1. / sd_rec['height'], 1. / sd_rec['width']] if not self.normalize_bbox: bbox_resize[0] *= float(self.image_min_side) bbox_resize[1] *= float(self.image_max_side) # Create labels for all boxes that are visible for box in boxes: # Add labels to boxes if box.name in self.classes: box.label = self.classes[box.name] # Check if box is visible and transform box to 1D vector if box_in_image(box=box, intrinsic=camera_intrinsic, imsize=imsize_src, vis_level=BoxVisibility.ANY): ## Points in box method for annotation filterS # check if bounding box has an according radar point if self.only_radar_annotated == 2: pcs, times = RadarPointCloud.from_file_multisweep(self.nusc, sample, self.radar_sensors[0], \ selected_sensor_channel, nsweeps=self.n_sweeps, min_distance=0.0, merge=False) for pc in pcs: pc.points = radar.enrich_radar_data(pc.points) if len(pcs) > 0: radar_sample = np.concatenate( [pc.points for pc in pcs], axis=-1) else: print( "[WARNING] only_radar_annotated=2 and sweeps=0 removes all annotations" ) radar_sample = np.zeros( shape=(len(radar.channel_map), 0)) radar_sample = radar_sample.astype( dtype=np.float32) mask = points_in_box(box, radar_sample[0:3, :]) if True not in mask: continue # If visible, we create the corresponding label box2d = box.box2d(camera_intrinsic ) # returns [xmin, ymin, xmax, ymax] box2d[0] *= bbox_resize[1] box2d[1] *= bbox_resize[0] box2d[2] *= bbox_resize[1] box2d[3] *= bbox_resize[0] annotations['bboxes'].insert(annotations_count, box2d) annotations['labels'].insert(annotations_count, box.label) annotations['num_radar_pts'].insert( annotations_count, self.nusc.get('sample_annotation', box.token)['num_radar_pts']) distance = (box.center[0]**2 + box.center[1]**2 + box.center[2]**2)**0.5 annotations['distances'].insert( annotations_count, distance) annotations['visibilities'].insert( annotations_count, int( self.nusc.get('sample_annotation', box.token)['visibility_token'])) annotations_count += 1 else: # The current name has been ignored pass annotations['labels'] = np.array(annotations['labels']) annotations['bboxes'] = np.array(annotations['bboxes']) annotations['distances'] = np.array(annotations['distances']) annotations['num_radar_pts'] = np.array(annotations['num_radar_pts']) annotations['visibilities'] = np.array(annotations['visibilities']) # num_radar_pts mathod for annotation filter if self.only_radar_annotated == 1: anns_to_keep = np.where(annotations['num_radar_pts'])[0] for key in annotations: annotations[key] = annotations[key][anns_to_keep] return annotations
def load_image(self, image_index): """ Returns the image plus from given image and radar samples. It takes the requested channels into account. :param sample_token: [str] the token pointing to a certain sample :returns: imageplus """ # Initialize local variables radar_name = self.radar_sensors[0] camera_name = self.camera_sensors[0] # Gettign data from nuscenes database sample_token = self.sample_tokens[image_index] sample = self.nusc.get('sample', sample_token) # Grab the front camera and the radar sensor. radar_token = sample['data'][radar_name] camera_token = sample['data'][camera_name] image_target_shape = (self.image_min_side, self.image_max_side) # Load the image image_sample = self.load_sample_data(sample, camera_name) # Add noise to the image if enabled if self.noisy_image_method is not None and self.noise_factor > 0: image_sample = noisy(self.noisy_image_method, image_sample, self.noise_factor) if self._is_image_plus_enabled() or self.camera_dropout > 0.0: # Parameters kwargs = { 'pointsensor_token': radar_token, 'camera_token': camera_token, 'height': (0, self.radar_projection_height), 'image_target_shape': image_target_shape, 'clear_radar': np.random.rand() < self.radar_dropout, 'clear_image': np.random.rand() < self.camera_dropout, } # Create image plus # radar_sample = self.load_sample_data(sample, radar_name) # Load samples from disk # Get filepath if self.noise_filter: required_sweep_count = self.n_sweeps + self.noise_filter.num_sweeps_required - 1 else: required_sweep_count = self.n_sweeps # sd_rec = self.nusc.get('sample_data', sample['data'][sensor_channel]) sensor_channel = radar_name pcs, times = RadarPointCloud.from_file_multisweep(self.nusc, sample, sensor_channel, \ sensor_channel, nsweeps=required_sweep_count, min_distance=0.0, merge=False) if self.noise_filter: # fill up with zero sweeps for _ in range(required_sweep_count - len(pcs)): pcs.insert( 0, RadarPointCloud( np.zeros(shape=(RadarPointCloud.nbr_dims(), 0)))) radar_sample = [radar.enrich_radar_data(pc.points) for pc in pcs] if self.noise_filter: ##### Filter the pcs ##### radar_sample = list( self.noise_filter.denoise(radar_sample, self.n_sweeps)) if len(radar_sample) == 0: radar_sample = np.zeros(shape=(len(radar.channel_map), 0)) else: ##### merge pcs into single radar samples array ##### radar_sample = np.concatenate(radar_sample, axis=-1) radar_sample = radar_sample.astype(dtype=np.float32) if self.perfect_noise_filter: cartesian_uncertainty = 0.5 # meters angular_uncertainty = math.radians(1.7) # degree category_selection = self.noise_category_selection nusc_sample_data = self.nusc.get('sample_data', radar_token) radar_gt_mask = calc_mask(nusc=self.nusc, nusc_sample_data=nusc_sample_data, points3d=radar_sample[0:3,:], \ tolerance=cartesian_uncertainty, angle_tolerance=angular_uncertainty, \ category_selection=category_selection) # radar_sample = radar_sample[:, radar_gt_mask.astype(np.bool)] radar_sample = np.compress(radar_gt_mask, radar_sample, axis=-1) if self.normalize_radar: # we need to noramlize # : use preprocess method analog to image preprocessing sigma_factor = int(self.normalize_radar) for ch in range( 3, radar_sample.shape[0] ): # neural fusion requires x y and z to be not normalized norm_interval = ( -127.5, 127.5 ) # caffee mode is default and has these norm interval for img radar_sample[ch, :] = radar.normalize( ch, radar_sample[ch, :], normalization_interval=norm_interval, sigma_factor=sigma_factor) img_p_full = self.image_plus_creation(self.nusc, image_data=image_sample, radar_data=radar_sample, **kwargs) # reduce to requested channels #self.channels = [ch - 1 for ch in self.channels] # Shift channels by 1, cause we have a weird convetion starting at 1 input_data = img_p_full[:, :, self.channels] else: # We are not in image_plus mode # Only resize, because in the other case this is contained in image_plus_creation input_data = cv2.resize(image_sample, image_target_shape[::-1]) return input_data
def load_image(self, image_index): """ Returns the image plus from given image and radar samples. It takes the requested channels into account. :param sample_token: [str] the token pointing to a certain sample :returns: imageplus """ # Initialize local variables radar_name = self.radar_sensors[0] camera_name = self.camera_sensors[0] # Gettign data from nuscenes database sample_token = self.sample_tokens[image_index] sample = self.nusc.get('sample', sample_token) # Grab the front camera and the radar sensor. radar_token = sample['data'][radar_name] camera_token = sample['data'][camera_name] image_target_shape = (self.image_min_side, self.image_max_side) # Load the image image_sample = self.load_sample_data(sample, camera_name, dtype=np.uint8) # Add noise to the image if enabled if self.noisy_image_method is not None and self.noise_factor > 0: image_sample = noisy(self.noisy_image_method, image_sample, self.noise_factor) if self.image_radar_fusion or self.camera_dropout > 0.0: # Parameters kwargs = { 'pointsensor_token': radar_token, 'camera_token': camera_token, 'image_target_shape': image_target_shape, 'clear_radar': np.random.rand() < self.radar_dropout, 'clear_image': np.random.rand() < self.camera_dropout, } # Create image plus # radar_sample = self.load_sample_data(sample, radar_name) # Load samples from disk # Get filepath if self.noise_filter: #TODO:现在的noise_filter还没有实现,等待以后实现,所以self.noise_filter是置0的 required_sweep_count = self.n_sweeps + self.noise_filter.num_sweeps_required - 1 else: required_sweep_count = self.n_sweeps # sd_rec = self.nusc.get('sample_data', sample['data'][sensor_channel]) sensor_channel = radar_name pcs, times = RadarPointCloud.from_file_multisweep(self.nusc, sample, sensor_channel, \ sensor_channel, nsweeps=required_sweep_count, min_distance=0.0) # if self.noise_filter: # # fill up with zero sweeps # for _ in range(required_sweep_count - len(pcs)): # pcs.insert(0, RadarPointCloud(np.zeros(shape=(RadarPointCloud.nbr_dims(), 0)))) radar_sample = pcs.points #[radar.enrich_radar_data(pc.points) for pc in pcs] # if self.noise_filter: # ##### Filter the pcs ##### # radar_sample = list(self.noise_filter.denoise(radar_sample, self.n_sweeps)) #如果self.noise_filter这个函数做好了,则下面注释的可以释放 # if len(radar_sample) == 0: # radar_sample = np.zeros(shape=(len(radar.channel_map), 0)) # else: # ##### merge pcs into single radar samples array ##### # radar_sample = np.concatenate(radar_sample, axis=-1) radar_sample = radar_sample.astype(dtype=np.float32) radar_sample = radar.enrich_radar_data( radar_sample) #给多次扫描的radar增加测量数据 if self.perfect_noise_filter: cartesian_uncertainty = 0.5 # meters angular_uncertainty = math.radians(1.7) # degree category_selection = self.noise_category_selection nusc_sample_data = self.nusc.get('sample_data', radar_token) radar_gt_mask = calc_mask(nusc=self.nusc, nusc_sample_data=nusc_sample_data, points3d=radar_sample[0:3, :], \ tolerance=cartesian_uncertainty, angle_tolerance=angular_uncertainty, \ category_selection=category_selection) # radar_sample = radar_sample[:, radar_gt_mask.astype(np.bool)] radar_sample = np.compress(radar_gt_mask, radar_sample, axis=-1) if self.normalize_radar: # we need to noramlize # : use preprocess method analog to image preprocessing sigma_factor = int(self.normalize_radar) for ch in range( 3, radar_sample.shape[0] ): # neural fusion requires x y and z to be not normalized norm_interval = ( 0, 255 ) # caffee mode is default and has these norm interval for img radar_sample[ch, :] = radar.normalize( ch, radar_sample[ch, :], normalization_interval=norm_interval, sigma_factor=sigma_factor) #print(radar_sample.shape) img_p_full, large_scale_p_full, middle_scale_p_full, little_scale_p_full = self.image_plus_creation( self.nusc, image_data=image_sample, radar_data=radar_sample, radar_cover_shape=self.radar_cover_shape, **kwargs) img_p_full = img_p_full.astype(np.uint8) large_scale_p_full = large_scale_p_full.astype(np.uint8) middle_scale_p_full = middle_scale_p_full.astype(np.uint8) little_scale_p_full = little_scale_p_full.astype(np.uint8) #如果image_plus_visual等于true时,则进行渲染 if self.image_plus_visual and self.isdebug: #在不debug时,程序不会进行渲染 #######################img_p_full#################################### input_data = self.create_input_data_plus_visual(img_p_full) #######################large_scale_p_full############################ large_scale_input_data = self.create_input_data_plus_visual( large_scale_p_full) #######################middle_scale_p_full########################### middle_scale_input_data = self.create_input_data_plus_visual( middle_scale_p_full) #######################little_scale_p_full########################### little_scale_input_data = self.create_input_data_plus_visual( little_scale_p_full) else: input_data = img_p_full[:, :, self.channels] large_scale_input_data = large_scale_p_full[:, :, self.channels] middle_scale_input_data = middle_scale_p_full[:, :, self.channels] little_scale_input_data = little_scale_p_full[:, :, self.channels] else: # We are not in image_plus mode # Only resize, because in the other case this is contained in image_plus_creation input_data = cv2.resize(image_sample, image_target_shape[::-1]) return input_data, large_scale_input_data, middle_scale_input_data, little_scale_input_data
def dataset_mapper(self, frame): """ Add sensor data in vehicle or global coordinates to the frame :param frame (dict): A frame dictionary from the db (no sensor data) :return frame (dict): frame with all sensor data """ sample_record = self.get('sample', frame['sample_token']) ref_sd_record = self.get( 'sample_data', sample_record['data'][self.cfg.REF_POSE_CHANNEL]) ref_pose_record = self.get('ego_pose', ref_sd_record['ego_pose_token']) ref_cs_record = self.get('calibrated_sensor', ref_sd_record['calibrated_sensor_token']) frame['ref_pose_record'] = ref_pose_record ## Load camera data cams = frame.get('camera', []) for cam in cams: sd_record = self.get('sample_data', cam['token']) filename = osp.join(self.dataroot, sd_record['filename']) image = self.get_camera_data(filename) cam['image'] = image cam['height'] = sd_record['height'] cam['width'] = sd_record['width'] cam['filename'] = filename cam['cs_record'] = self.get('calibrated_sensor', sd_record['calibrated_sensor_token']) cam['pose_record'] = self.get('ego_pose', sd_record['ego_pose_token']) ## Load annotations in vehicle coordinates frame['anns'] = self._get_anns(frame['anns'], ref_pose_record) ## Filter anns outside image if in 'camera' mode: if self.cfg.SAMPLE_MODE == 'camera': filtered_anns = [] for ann in frame['anns']: box_cam = nsutils.map_annotation_to_camera( ann['box_3d'], cam['cs_record'], cam['pose_record'], ref_pose_record, self.cfg.COORDINATES) cam_intrinsic = np.array(cam['cs_record']['camera_intrinsic']) if not box_in_image(box_cam, cam_intrinsic, (1600, 900)): continue if self.cfg.GEN_2D_BBOX: ann['box_2d'] = nsutils.box_3d_to_2d_simple( box_cam, cam_intrinsic, (1600, 900)) filtered_anns.append(ann) frame['anns'] = filtered_anns ## Load LIDAR data in vehicle coordinates if 'lidar' in frame: lidar = frame['lidar'] sd_record = self.get('sample_data', lidar['token']) lidar['cs_record'] = self.get('calibrated_sensor', sd_record['calibrated_sensor_token']) lidar['pose_record'] = self.get('ego_pose', sd_record['ego_pose_token']) lidar_pc, _ = LidarPointCloud.from_file_multisweep( self, sample_record, lidar['channel'], lidar['channel'], nsweeps=self.cfg.LIDAR_SWEEPS, min_distance=self.cfg.PC_MIN_DIST) ## Take from sensor to vehicle coordinates lidar_pc = nsutils.sensor_to_vehicle(lidar_pc, lidar['cs_record']) ## filter points outside the image if in 'camera' mode if self.cfg.SAMPLE_MODE == "camera": cam = frame['camera'][0] cam_intrinsic = np.array(cam['cs_record']['camera_intrinsic']) lidar_pc_cam, _ = nsutils.map_pointcloud_to_camera( lidar_pc, cam_cs_record=cam['cs_record'], cam_pose_record=cam['pose_record'], pointsensor_pose_record=frame['lidar']['pose_record'], coordinates=frame['coordinates']) _, _, mask = nsutils.map_pointcloud_to_image(lidar_pc_cam, cam_intrinsic, img_shape=(1600, 900)) lidar_pc.points = lidar_pc.points[:, mask] frame['lidar']['pointcloud'] = lidar_pc ## Load Radar data in vehicle coordinates if 'radar' in frame: all_radar_pcs = RadarPointCloud(np.zeros((18, 0))) for radar in frame['radar']: radar_pc, _ = RadarPointCloud.from_file_multisweep( self, sample_record, radar['channel'], self.cfg.REF_POSE_CHANNEL, nsweeps=self.cfg.RADAR_SWEEPS, min_distance=self.cfg.PC_MIN_DIST) radar_pc = nsutils.sensor_to_vehicle(radar_pc, ref_cs_record) ## filter points outside the image if in 'camera' mode if self.cfg.SAMPLE_MODE == "camera": cam = frame['camera'][0] cam_intrinsic = np.array( cam['cs_record']['camera_intrinsic']) radar_pc_cam, _ = nsutils.map_pointcloud_to_camera( radar_pc, cam_cs_record=cam['cs_record'], cam_pose_record=cam['pose_record'], pointsensor_pose_record=ref_pose_record, coordinates=frame['coordinates']) _, _, mask = nsutils.map_pointcloud_to_image( radar_pc_cam, cam_intrinsic, img_shape=(1600, 900)) radar_pc.points = radar_pc.points[:, mask] all_radar_pcs.points = np.hstack( (all_radar_pcs.points, radar_pc.points)) frame['radar'] = {} frame['radar']['pointcloud'] = all_radar_pcs frame['radar']['pose_record'] = ref_pose_record frame['radar']['cs_record'] = ref_cs_record return frame
def nuscenes_gt_to_kitti(self) -> None: """ Converts nuScenes GT annotations to KITTI format. """ kitti_to_nu_lidar = Quaternion(axis=(0, 0, 1), angle=np.pi / 2) kitti_to_nu_lidar_inv = kitti_to_nu_lidar.inverse imsize = (1600, 900) token_idx = 0 # Start tokens from 0. # Create output folders. label_folder = os.path.join(self.output_dir, self.split, 'label_2') calib_folder = os.path.join(self.output_dir, self.split, 'calib') image_folder = os.path.join(self.output_dir, self.split, 'image_2') lidar_folder = os.path.join(self.output_dir, self.split, 'velodyne') radar_folder = os.path.join(self.output_dir, self.split, 'radar') for folder in [ label_folder, calib_folder, image_folder, lidar_folder, radar_folder ]: if not os.path.isdir(folder): os.makedirs(folder) id_to_token_file = os.path.join(self.output_dir, self.split, 'id2token.txt') id2token = open(id_to_token_file, "w+") # Use only the samples from the current split. split_logs = create_splits_logs(self.split, self.nusc) sample_tokens = self._split_to_samples(split_logs) sample_tokens = sample_tokens[:self.image_count] out_filenames = [] for sample_token in tqdm(sample_tokens): # Get sample data. sample = self.nusc.get('sample', sample_token) sample_annotation_tokens = sample['anns'] cam_token = sample['data'][self.cam_name] lidar_token = sample['data'][self.lidar_name] radar_tokens = [] for radar_name in _C.RADARS.keys(): radar_tokens.append(sample['data'][radar_name]) # Retrieve sensor records. sd_record_cam = self.nusc.get('sample_data', cam_token) sd_record_lid = self.nusc.get('sample_data', lidar_token) cs_record_cam = self.nusc.get( 'calibrated_sensor', sd_record_cam['calibrated_sensor_token']) cs_record_lid = self.nusc.get( 'calibrated_sensor', sd_record_lid['calibrated_sensor_token']) sd_record_rad = [] cs_record_rad = [] for i, radar_token in enumerate(radar_tokens): sd_record_rad.append(self.nusc.get('sample_data', radar_token)) cs_record_rad.append( self.nusc.get('calibrated_sensor', sd_record_rad[i]['calibrated_sensor_token'])) # Combine transformations and convert to KITTI format. # Note: cam uses same conventions in KITTI and nuScenes. lid_to_ego = transform_matrix(cs_record_lid['translation'], Quaternion( cs_record_lid['rotation']), inverse=False) ego_to_cam = transform_matrix(cs_record_cam['translation'], Quaternion( cs_record_cam['rotation']), inverse=True) rad_to_ego = [] for cs_rec_rad in cs_record_rad: rad_to_ego.append( transform_matrix(cs_rec_rad['translation'], Quaternion(cs_rec_rad['rotation']), inverse=False)) velo_to_cam = np.dot(ego_to_cam, lid_to_ego) # # TODO: Assuming Radar point are going to be in ego coordinates # radar_to_cam = ego_to_cam # Convert from KITTI to nuScenes LIDAR coordinates, where we apply velo_to_cam. velo_to_cam_kitti = np.dot(velo_to_cam, kitti_to_nu_lidar.transformation_matrix) # # Nuscenes radars use same convention as KITTI lidar # radar_to_cam_kitti = radar_to_cam # Currently not used. imu_to_velo_kitti = np.zeros((3, 4)) # Dummy values. r0_rect = Quaternion(axis=[1, 0, 0], angle=0) # Dummy values. # Projection matrix. p_left_kitti = np.zeros((3, 4)) # Cameras are always rectified. p_left_kitti[:3, :3] = cs_record_cam['camera_intrinsic'] # Create KITTI style transforms. velo_to_cam_rot = velo_to_cam_kitti[:3, :3] velo_to_cam_trans = velo_to_cam_kitti[:3, 3] # radar_to_cam_rot = radar_to_cam_kitti[:3, :3] # radar_to_cam_trans = radar_to_cam_kitti[:3, 3] # Check that the lidar rotation has the same format as in KITTI. assert (velo_to_cam_rot.round(0) == np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]])).all() assert (velo_to_cam_trans[1:3] < 0).all() # Retrieve the token from the lidar. # Note that this may be confusing as the filename of the camera will # include the timestamp of the lidar, not the camera. filename_cam_full = sd_record_cam['filename'] filename_lid_full = sd_record_lid['filename'] filename_rad_full = [] for sd_rec_rad in sd_record_rad: filename_rad_full.append(sd_rec_rad['filename']) out_filename = '%06d' % token_idx # Alternative to use KITTI names. # out_filename = sample_token # Write token to disk. text = sample_token id2token.write(text + '\n') id2token.flush() token_idx += 1 # Convert image (jpg to png). src_im_path = os.path.join(self.nusc.dataroot, filename_cam_full) dst_im_path = os.path.join(image_folder, out_filename + '.png') if self.use_symlinks: # Create symbolic links to nuscenes images os.symlink(os.path.abspath(src_im_path), dst_im_path) else: im = Image.open(src_im_path) im.save(dst_im_path, "PNG") # Convert lidar. src_lid_path = os.path.join(self.nusc.dataroot, filename_lid_full) dst_lid_path = os.path.join(lidar_folder, out_filename + '.bin') assert not dst_lid_path.endswith('.pcd.bin') # pcl = LidarPointCloud.from_file(src_lid_path) pcl, _ = LidarPointCloud.from_file_multisweep( nusc=self.nusc, sample_rec=sample, chan=self.lidar_name, ref_chan=self.lidar_name, nsweeps=self.lidar_sweeps, min_distance=1) pcl.rotate( kitti_to_nu_lidar_inv.rotation_matrix) # In KITTI lidar frame. pcl.points = pcl.points.astype('float32') with open(dst_lid_path, "w") as lid_file: pcl.points.T.tofile(lid_file) # # Visualize pointclouds # _, ax = plt.subplots(1, 1, figsize=(9, 9)) # points = view_points(pcl.points[:3, :], np.eye(4), normalize=False) # dists = np.sqrt(np.sum(pcl.points[:2, :] ** 2, axis=0)) # colors = np.minimum(1, dists / 50) # ax.scatter(points[0, :], points[1, :], c=colors, s=0.2) # # plt.show(block=False) # plt.show() # Convert radar. src_rad_path = [] for filename_radar in filename_rad_full: src_rad_path.append( os.path.join(self.nusc.dataroot, filename_radar)) dst_rad_path = os.path.join(radar_folder, out_filename + '.bin') assert not dst_rad_path.endswith('.pcd.bin') pcl = RadarPointCloud(np.zeros((18, 0))) ## Get Radar points in Lidar coordinate system for i, rad_path in enumerate(src_rad_path): pc, _ = RadarPointCloud.from_file_multisweep( self.nusc, sample_rec=sample, chan=sd_record_rad[i]['channel'], ref_chan=self.lidar_name, nsweeps=self.radar_sweeps, min_distance=0) # rot_matrix = Quaternion(cs_record_rad[i]['rotation']).rotation_matrix # pc.rotate(rot_matrix) # pc.translate(np.array(cs_record_rad[i]['translation'])) pcl.points = np.hstack((pcl.points, pc.points)) pcl.rotate( kitti_to_nu_lidar_inv.rotation_matrix) # In KITTI lidar frame. ## Visualize pointclouds # _, ax = plt.subplots(1, 1, figsize=(9, 9)) # points = view_points(pcl.points[:3, :], np.eye(4), normalize=False) # dists = np.sqrt(np.sum(pcl.points[:2, :] ** 2, axis=0)) # colors = np.minimum(1, dists / 50) # ax.scatter(points[0, :], points[1, :], c=colors, s=0.2) # plt.show() pcl.points = pcl.points.astype('float32') with open(dst_rad_path, "w") as rad_file: pcl.points.T.tofile(rad_file) # Add to tokens. out_filenames.append(out_filename) # Create calibration file. kitti_transforms = dict() kitti_transforms['P0'] = np.zeros((3, 4)) # Dummy values. kitti_transforms['P1'] = np.zeros((3, 4)) # Dummy values. kitti_transforms['P2'] = p_left_kitti # Left camera transform. kitti_transforms['P3'] = np.zeros((3, 4)) # Dummy values. kitti_transforms[ 'R0_rect'] = r0_rect.rotation_matrix # Cameras are already rectified. kitti_transforms['Tr_velo_to_cam'] = np.hstack( (velo_to_cam_rot, velo_to_cam_trans.reshape(3, 1))) # kitti_transforms['Tr_radar_to_cam'] = np.hstack((radar_to_cam_rot, radar_to_cam_trans.reshape(3, 1))) kitti_transforms['Tr_imu_to_velo'] = imu_to_velo_kitti calib_path = os.path.join(calib_folder, out_filename + '.txt') with open(calib_path, "w") as calib_file: for (key, val) in kitti_transforms.items(): val = val.flatten() val_str = '%.12e' % val[0] for v in val[1:]: val_str += ' %.12e' % v calib_file.write('%s: %s\n' % (key, val_str)) # Write label file. label_path = os.path.join(label_folder, out_filename + '.txt') if os.path.exists(label_path): print('Skipping existing file: %s' % label_path) continue with open(label_path, "w") as label_file: for sample_annotation_token in sample_annotation_tokens: sample_annotation = self.nusc.get('sample_annotation', sample_annotation_token) # Get box in LIDAR frame. _, box_lidar_nusc, _ = self.nusc.get_sample_data( lidar_token, box_vis_level=BoxVisibility.NONE, selected_anntokens=[sample_annotation_token]) box_lidar_nusc = box_lidar_nusc[0] # Truncated: Set all objects to 0 which means untruncated. truncated = 0.0 # Occluded: Set all objects to full visibility as this information is not available in nuScenes. occluded = 0 # Convert nuScenes category to nuScenes detection challenge category. detection_name = _C.KITTI_CLASSES.get( sample_annotation['category_name']) # Skip categories that are not in the KITTI classes. if detection_name is None: continue # Convert from nuScenes to KITTI box format. box_cam_kitti = KittiDB.box_nuscenes_to_kitti( box_lidar_nusc, Quaternion(matrix=velo_to_cam_rot), velo_to_cam_trans, r0_rect) # Project 3d box to 2d box in image, ignore box if it does not fall inside. bbox_2d = KittiDB.project_kitti_box_to_image(box_cam_kitti, p_left_kitti, imsize=imsize) if bbox_2d is None: # continue ## If box is not inside the image, 2D boxes are set to zero bbox_2d = (0, 0, 0, 0) # Set dummy score so we can use this file as result. box_cam_kitti.score = 0 # Convert box to output string format. output = KittiDB.box_to_string(name=detection_name, box=box_cam_kitti, bbox_2d=bbox_2d, truncation=truncated, occlusion=occluded) # Write to disk. label_file.write(output + '\n') id2token.close()