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_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 getPoints(self, index): sample = self.nusc.get('sample', self.filtered_sample_tokens[index]) sample_lidar_token = sample['data']['LIDAR_TOP'] lidar_data = self.nusc.get('sample_data', sample_lidar_token) ego_pose = self.nusc.get('ego_pose', lidar_data['ego_pose_token']) calibrated_sensor = self.nusc.get( 'calibrated_sensor', lidar_data['calibrated_sensor_token']) global_from_car = transform_matrix(ego_pose['translation'], Quaternion(ego_pose['rotation']), inverse=False) car_from_sensor = transform_matrix(calibrated_sensor['translation'], Quaternion( calibrated_sensor['rotation']), inverse=False) try: lidar_pointcloud, times = LidarPointCloud.from_file_multisweep( self.nusc, sample, 'LIDAR_TOP', 'LIDAR_TOP') lidar_pointcloud.transform(car_from_sensor) except Exception as e: print(f"Failed to load Lidar Pointcloud for {sample}:{e}") points = lidar_pointcloud.points points[3, :] /= 255 points[3, :] -= 0.5 # points_cat = np.concatenate([points, times], axis=0).transpose() points_cat = points.transpose() points_cat = points_cat[~np.isnan(points_cat).any(axis=1)] return points_cat
def get_points_data(nusc, all_sample_tokens): all_points = [] all_ego_poses = [] for sample_token in all_sample_tokens: sample = nusc.get('sample', sample_token) lidar_data = nusc.get('sample_data', sample['data']['LIDAR_TOP']) chan = 'LIDAR_TOP' ref_chan = 'LIDAR_TOP' pc, _ = LidarPointCloud.from_file_multisweep(nusc, sample, chan, ref_chan, nsweeps=10) # First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep. cs_record = nusc.get('calibrated_sensor', lidar_data['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. ps_record = nusc.get('ego_pose', lidar_data['ego_pose_token']) pc.rotate(Quaternion(ps_record['rotation']).rotation_matrix) pc.translate(np.array(ps_record['translation'])) points = pc.points points[3, :] = points[3, :] / 255.0 all_points.append(points) all_ego_poses.append(ps_record) return all_points, all_ego_poses
def get_lidar_pointcloud(self): """ Extracting lidar pointcloud for current timestep in current scene Author: Javier :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) #calibration of LiDAR points ref_sd_record = self.nusc.get('sample_data', sample_data_token) cs_record = self.nusc.get('calibrated_sensor', ref_sd_record['calibrated_sensor_token']) pose_record = self.nusc.get('ego_pose', ref_sd_record['ego_pose_token']) ref_to_ego = transform_matrix(translation=cs_record['translation'], rotation=Quaternion(cs_record["rotation"])) # Compute rotation between 3D vehicle pose and "flat" vehicle pose (parallel to global z plane). ego_yaw = Quaternion(pose_record['rotation']).yaw_pitch_roll[0] rotation_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] = rotation_vehicle_flat_from_vehicle viewpoint = np.dot(vehicle_flat_from_vehicle, ref_to_ego) points = view_points(pc.points[:3, :], viewpoint, normalize=False) self.points_lidar = points return points
def get_lidar_pointcloud(self, nsweeps=5): """ Extracting lidar pointcloud for current timestep in current scene :return Point cloud [Position(x,y,z,i,t) 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) points = np.concatenate([pc.points, times], axis=0) return points
def get_lidar_instances(self, nsweeps=1, enlarge=1): channel = 'LIDAR_TOP' sample_data_token = self.sample['data'][channel] sd_record = self.nusc.get('sample_data', sample_data_token) _, boxes, _ = self.nusc.get_sample_data( sample_data_token, use_flat_vehicle_coordinates=False) # Get aggregated point cloud in lidar frame. chan = sd_record['channel'] pc, _ = LidarPointCloud.from_file_multisweep(self.nusc, self.sample, chan, channel, nsweeps) points = pc.points anno_points = [] anno_boxes = [] for box in boxes: mask = points_in_box(box, points[0:3, :], wlh_factor=enlarge) if True in mask: anno_points.append(points[:, mask]) anno_boxes.append(box) return anno_points, anno_boxes
def _get_lidar_data(self, sample_rec: dict, sample_data: dict, pose_rec: dict, nsweeps: int = 1) -> LidarPointCloud: """ Returns the LIDAR pointcloud for this frame in vehicle/global coordniates :param sample_rec: sample record dictionary from nuscenes :param sample_data: sample data dictionary from nuscenes :param pose_rec: ego pose record dictionary from nuscenes :param nsweeps: number of sweeps to return for the LIDAR :return: LidarPointCloud containing this sample and all sweeps """ lidar_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 lidar_pc, _ = LidarPointCloud.from_file_multisweep( self.nusc, sample_rec, sample_data['channel'], sample_data['channel'], nsweeps=nsweeps) else: ## returns in sensor coordinates lidar_pc = LidarPointCloud.from_file(lidar_path) ## Sensor to vehicle lidar_pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix) lidar_pc.translate(np.array(cs_record['translation'])) ## Vehicle to global if self.coordinates == 'global': lidar_pc.rotate(Quaternion(pose_rec['rotation']).rotation_matrix) lidar_pc.translate(np.array(pose_rec['translation'])) return lidar_pc, cs_record
def get_pcl(): nusc = NuScenes(version='v1.0-mini', dataroot='/data/sets/nuscenes', verbose=True) explorer = NuScenesExplorer(nusc) imglist = [] count = 0 for scene in nusc.scene: token = scene['first_sample_token'] while token != '': count += 1 sample = nusc.get('sample',token) sample_data_cam = nusc.get('sample_data', sample['data']['CAM_FRONT']) sample_data_pcl = nusc.get('sample_data', sample['data']['LIDAR_TOP']) # get CAM_FRONT datapath data_path, boxes, camera_intrinsic = explorer.nusc.get_sample_data(sample_data_cam['token'], box_vis_level = BoxVisibility.ANY) imglist += [data_path] # get LiDAR point cloud sample_rec = explorer.nusc.get('sample', sample_data_pcl['sample_token']) chan = sample_data_pcl['channel'] ref_chan = 'LIDAR_TOP' pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan) radius = np.sqrt((pc.points[0])**2 + (pc.points[1])**2 + (pc.points[2])**2) pc = pc.points.transpose() pc = pc[np.where(radius<20)] print(pc) display(pc.transpose(), count) token = sample['next'] if count == 2: break if count == 2: break plt.show() print(len(imglist))
def get_each_sweep_lidar_pointcloud(self, nsweeps=5): """ Extracting lidar pointcloud for current timestep in current scene :return Point cloud [[Position(x,y,z, a) X n_points] * timestamps] Time stamps a list of int for timestamps in microsecond """ # 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) times_interval = np.unique(times)[::-1] unique_times = self.get_timestamp() - (times_interval * 1e6).astype( np.int) points = [] for t in times_interval: points.append(pc.points[:, times[0] == t]) return points, unique_times
def get_lidar_points(lyftdata, 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, nsweeps=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 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 visualize_sample(nusc: NuScenes, sample_token: str, gt_boxes: EvalBoxes, pred_boxes: EvalBoxes, nsweeps: int = 1, conf_th: float = 0.15, eval_range: float = 50, verbose: bool = True, savepath: str = None) -> None: """ Visualizes a sample from BEV with annotations and detection results. :param nusc: NuScenes object. :param sample_token: The nuScenes sample token. :param gt_boxes: Ground truth boxes grouped by sample. :param pred_boxes: Prediction grouped by sample. :param nsweeps: Number of sweeps used for lidar visualization. :param conf_th: The confidence threshold used to filter negatives. :param eval_range: Range in meters beyond which boxes are ignored. :param verbose: Whether to print to stdout. :param savepath: If given, saves the the rendering here instead of displaying. """ # Retrieve sensor & pose records. sample_rec = nusc.get('sample', sample_token) sd_record = nusc.get('sample_data', sample_rec['data']['LIDAR_TOP']) cs_record = nusc.get('calibrated_sensor', sd_record['calibrated_sensor_token']) pose_record = nusc.get('ego_pose', sd_record['ego_pose_token']) # Get boxes. boxes_gt_global = gt_boxes[sample_token] boxes_est_global = pred_boxes[sample_token] # Map GT boxes to lidar. boxes_gt = boxes_to_sensor(boxes_gt_global, pose_record, cs_record) # Map EST boxes to lidar. boxes_est = boxes_to_sensor(boxes_est_global, pose_record, cs_record) # Add scores to EST boxes. for box_est, box_est_global in zip(boxes_est, boxes_est_global): box_est.score = box_est_global.detection_score # Get point cloud in lidar frame. pc, _ = LidarPointCloud.from_file_multisweep(nusc, sample_rec, 'LIDAR_TOP', 'LIDAR_TOP', nsweeps=nsweeps) # Init axes. _, 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 / eval_range) ax.scatter(points[0, :], points[1, :], c=colors, s=0.2) # Show ego vehicle. ax.plot(0, 0, 'x', color='black') # Show GT boxes. for box in boxes_gt: box.render(ax, view=np.eye(4), colors=('g', 'g', 'g'), linewidth=2) # Show EST boxes. for box in boxes_est: # Show only predictions with a high score. assert not np.isnan(box.score), 'Error: Box score cannot be NaN!' if box.score >= conf_th: box.render(ax, view=np.eye(4), colors=('b', 'b', 'b'), linewidth=1) # Limit visible range. axes_limit = eval_range + 3 # Slightly bigger to include boxes that extend beyond the range. ax.set_xlim(-axes_limit, axes_limit) ax.set_ylim(-axes_limit, axes_limit) # Show / save plot. if verbose: print('Rendering sample token %s' % sample_token) plt.title(sample_token) if savepath is not None: plt.savefig(savepath) plt.close() else: plt.show()
def create_dataset(dataroot, save_dir, pretrained_model, width=672, height=672, grid_range=70., nusc_version='v1.0-mini', use_constant_feature=True, use_intensity_feature=True, end_id=None): device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') bcnn_model = BCNN(in_channels=6, n_class=5).to(device) # bcnn_model = torch.nn.DataParallel(bcnn_model) # multi gpu if os.path.exists(pretrained_model): print('Use pretrained model') bcnn_model.load_state_dict( fix_model_state_dict(torch.load(pretrained_model))) bcnn_model.eval() else: return os.makedirs(os.path.join(save_dir, 'in_feature'), exist_ok=True) os.makedirs(os.path.join(save_dir, 'out_feature'), exist_ok=True) os.makedirs(os.path.join(save_dir, 'inference_feature'), exist_ok=True) nusc = NuScenes(version=nusc_version, dataroot=dataroot, verbose=True) ref_chan = 'LIDAR_TOP' if width == height: size = width else: raise Exception( 'Currently only supported if width and height are equal') grid_length = 2. * grid_range / size label_half_length = 0 data_id = 0 grid_ticks = np.arange(-grid_range, grid_range + grid_length, grid_length) grid_centers \ = (grid_ticks + grid_length / 2)[:len(grid_ticks) - 1] for my_scene in nusc.scene: first_sample_token = my_scene['first_sample_token'] token = first_sample_token # try: while (token != ''): print('--- {} '.format(data_id) + token + ' ---') my_sample = nusc.get('sample', token) sd_record = nusc.get('sample_data', my_sample['data'][ref_chan]) sample_rec = nusc.get('sample', sd_record['sample_token']) chan = sd_record['channel'] pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan, nsweeps=1) _, boxes, _ = nusc.get_sample_data(sd_record['token'], box_vis_level=0) out_feature = np.zeros((size, size, 8), dtype=np.float32) for box_idx, box in enumerate(boxes): label = 0 if box.name.split('.')[0] == 'vehicle': if box.name.split('.')[1] == 'car': label = 1 elif box.name.split('.')[1] == 'bus': label = 1 elif box.name.split('.')[1] == 'truck': label = 1 elif box.name.split('.')[1] == 'construction': label = 1 elif box.name.split('.')[1] == 'emergency': label = 1 elif box.name.split('.')[1] == 'trailer': label = 1 elif box.name.split('.')[1] == 'bicycle': label = 2 elif box.name.split('.')[1] == 'motorcycle': label = 2 elif box.name.split('.')[0] == 'human': label = 3 # elif box.name.split('.')[0] == 'movable_object': # label = 1 # elif box.name.split('.')[0] == 'static_object': # label = 1 else: continue height_pt = np.linalg.norm(box.corners().T[0] - box.corners().T[3]) corners2d = box.corners()[:2, :] box2d = corners2d.T[[2, 3, 7, 6]] box2d_center = box2d.mean(axis=0) yaw, pitch, roll = box.orientation.yaw_pitch_roll generate_out_feature(width, height, size, grid_centers, box2d, box2d_center, height_pt, label, label_half_length, yaw, out_feature) # import pdb; pdb.set_trace() out_feature = out_feature.astype(np.float16) in_feature_generator = Feature_generator(grid_range, width, height, use_constant_feature, use_intensity_feature) in_feature_generator.generate(pc.points.T) in_feature = in_feature_generator.feature if use_constant_feature and use_intensity_feature: in_feature = in_feature.reshape(size, size, 8) elif use_constant_feature or use_intensity_feature: in_feature = in_feature.reshape(size, size, 6) else: in_feature = in_feature.reshape(size, size, 4) # instance_pt is flipped due to flip # out_feature = np.flip(np.flip(out_feature, axis=0), axis=1) # out_feature[:, :, 1:3] *= -1 np.save(os.path.join(save_dir, 'in_feature/{:05}'.format(data_id)), in_feature) np.save( os.path.join(save_dir, 'out_feature/{:05}'.format(data_id)), out_feature) # inference feature # in_feature = torch.from_numpy(in_feature) in_feature = in_feature.astype(np.float32) transform = transforms.Compose([transforms.ToTensor()]) in_feature = transform(in_feature) in_feature = in_feature.to(device) in_feature = torch.unsqueeze(in_feature, 0) print(in_feature.size()) inference_feature = bcnn_model(in_feature) np.save( os.path.join(save_dir, 'inference_feature/{:05}'.format(data_id)), inference_feature.cpu().detach().numpy()) token = my_sample['next'] data_id += 1 if data_id == end_id: return
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')
#model.eval() im_token = image_token[0] sample_data = nusc.get('sample_data', im_token) sample_token = sample_data['sample_token'] sample =nusc.get('sample', sample_token) image_name = sample_data['filename'] img = imread('/data/sets/nuscenes/' + image_name) # load in the LiDAR data for the sample sample_data_pcl = nusc.get('sample_data', sample['data']['LIDAR_TOP']) sample_rec = explorer.nusc.get('sample', sample_data_pcl['sample_token']) # get LiDAR point cloud chan = sample_data_pcl['channel'] ref_chan = 'LIDAR_TOP' pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan) points= pc.points[:3,:] points = points.astype(np.float32, copy=False) points = np.expand_dims(points, axis=0) points= torch.from_numpy(points) pc2, c, im = map_pointcloud_to_image(sample_data_pcl['token'], im_token) pointfeat = PointNetfeat(global_feat=True) _, global_feat= pointfeat(points) #classifier = classifier.train() #global_feat = model(points) pdb.set_trace()
def create_dataset(dataroot, save_dir, width=672, height=672, grid_range=70., nusc_version='v1.0-mini', use_constant_feature=False, use_intensity_feature=True, end_id=None, augmentation_num=0, add_noise=False): """Create a learning dataset from Nuscens Parameters ---------- dataroot : str Nuscenes dataroot path. save_dir : str Dataset save directory. width : int, optional feature map width, by default 672 height : int, optional feature map height, by default 672 grid_range : float, optional feature map range, by default 70. nusc_version : str, optional Nuscenes version. v1.0-mini or v1.0-trainval, by default 'v1.0-mini' use_constant_feature : bool, optional Whether to use constant feature, by default False use_intensity_feature : bool, optional Whether to use intensity feature, by default True end_id : int, optional How many data to generate. If None, all data, by default None augmentation_num : int, optional How many data augmentations for one sample, by default 0 add_noise : bool, optional Whether to add noise to pointcloud, by default True Raises ------ Exception Width and height are not equal """ os.makedirs(os.path.join(save_dir, 'in_feature'), exist_ok=True) os.makedirs(os.path.join(save_dir, 'out_feature'), exist_ok=True) nusc = NuScenes(version=nusc_version, dataroot=dataroot, verbose=True) ref_chan = 'LIDAR_TOP' if width == height: size = width else: raise Exception( 'Currently only supported if width and height are equal') grid_length = 2. * grid_range / size z_trans_range = 0.5 sample_id = 0 data_id = 0 grid_ticks = np.arange(-grid_range, grid_range + grid_length, grid_length) grid_centers \ = (grid_ticks + grid_length / 2)[:len(grid_ticks) - 1] for my_scene in nusc.scene: first_sample_token = my_scene['first_sample_token'] token = first_sample_token # try: while (token != ''): print('sample:{} {} created_data={}'.format( sample_id, token, data_id)) my_sample = nusc.get('sample', token) sd_record = nusc.get('sample_data', my_sample['data'][ref_chan]) sample_rec = nusc.get('sample', sd_record['sample_token']) chan = sd_record['channel'] pc_raw, _ = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan, nsweeps=1) _, boxes_raw, _ = nusc.get_sample_data(sd_record['token'], box_vis_level=0) z_trans = 0 q = Quaternion() for augmentation_idx in range(augmentation_num + 1): pc = copy.copy(pc_raw) if add_noise: pc.points = add_noise_points(pc.points.T).T boxes = copy.copy(boxes_raw) if augmentation_idx > 0: z_trans = (np.random.rand() - 0.5) * 2 * z_trans_range pc.translate([0, 0, z_trans]) z_rad = np.random.rand() * np.pi * 2 q = Quaternion(axis=[0, 0, 1], radians=z_rad) pc.rotate(q.rotation_matrix) pc_points = pc.points.astype(np.float32) out_feature = np.zeros((size, size, 8), dtype=np.float32) for box_idx, box in enumerate(boxes): if augmentation_idx > 0: box.translate([0, 0, z_trans]) box.rotate(q) label = 0 if box.name.split('.')[0] == 'vehicle': if box.name.split('.')[1] == 'car': label = 1 elif box.name.split('.')[1] == 'bus': label = 2 elif box.name.split('.')[1] == 'truck': label = 2 elif box.name.split('.')[1] == 'construction': label = 2 elif box.name.split('.')[1] == 'emergency': label = 2 elif box.name.split('.')[1] == 'trailer': label = 2 elif box.name.split('.')[1] == 'bicycle': label = 3 elif box.name.split('.')[1] == 'motorcycle': label = 3 elif box.name.split('.')[0] == 'human': label = 4 # elif box.name.split('.')[0] == 'movable_object': # label = 1 # elif box.name.split('.')[0] == 'static_object': # label = 1 else: continue height_pt = np.linalg.norm(box.corners().T[0] - box.corners().T[3]) box_corners = box.corners().astype(np.float32) corners2d = box_corners[:2, :] box2d = corners2d.T[[2, 3, 7, 6]] box2d_center = box2d.mean(axis=0) yaw, pitch, roll = box.orientation.yaw_pitch_roll out_feature = generate_out_feature(size, grid_centers, box_corners, box2d, box2d_center, pc_points, height_pt, label, yaw, out_feature) # feature_generator = fg.FeatureGenerator( # grid_range, width, height, # use_constant_feature, use_intensity_feature) feature_generator = fgpb.FeatureGenerator( grid_range, size, size) in_feature = feature_generator.generate( pc_points.T, use_constant_feature, use_intensity_feature) if use_constant_feature and use_intensity_feature: channels = 8 elif use_constant_feature or use_intensity_feature: channels = 6 else: channels = 4 in_feature = np.array(in_feature).reshape( channels, size, size).astype(np.float16) in_feature = in_feature.transpose(1, 2, 0) np.save( os.path.join(save_dir, 'in_feature/{:05}'.format(data_id)), in_feature) np.save( os.path.join(save_dir, 'out_feature/{:05}'.format(data_id)), out_feature) token = my_sample['next'] data_id += 1 if data_id == end_id: return sample_id += 1
camera_token_FRONT_LEFT = sample_rec['data']['CAM_FRONT_LEFT'] camera_token_FRONT_RIGHT = sample_rec['data']['CAM_FRONT_RIGHT'] camera_token_BACK = sample_rec['data']['CAM_BACK'] pointsensor_token = sample_rec['data']['LIDAR_TOP'] pointsensor = nusc.get('sample_data', pointsensor_token) #get camera info cam = nusc.get('sample_data', camera_token) cam_front_left = nusc.get('sample_data', camera_token_FRONT_LEFT) cam_front_right = nusc.get('sample_data', camera_token_FRONT_RIGHT) cam_back = nusc.get('sample_data', camera_token_BACK) orig_pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, 'LIDAR_TOP', 'LIDAR_TOP', nsweeps=10) #get ego vehicle pose pose_record = nusc.get('ego_pose', pointsensor['ego_pose_token']) #get point cloud in camera frame prior to putting inside image plane pc, global_frame_pc = project_points_to_image(orig_pc, pointsensor, cam) pc_front_left, _ = project_points_to_image(orig_pc, pointsensor, cam_front_left) pc_front_right, _ = project_points_to_image(orig_pc, pointsensor, cam_front_right) pc_back, _ = project_points_to_image(orig_pc, pointsensor, cam_back)
def get_pointcloud(nusc, bottom_left, top_right, box, pointsensor_token: str, camera_token: str, min_dist: float = 1.0) -> Tuple: """ Given a point sensor (lidar/radar) token and camera sample_data token, load point-cloud and map it to the image plane. :param pointsensor_token: Lidar/radar sample_data token. :param camera_token: Camera sample_data token. :param min_dist: Distance from the camera below which points are discarded. :return (pointcloud <np.float: 2, n)>, coloring <np.float: n>, image <Image>). """ sample_data = nusc.get("sample_data", camera_token) explorer = NuScenesExplorer(nusc) cam = nusc.get('sample_data', camera_token) pointsensor = nusc.get('sample_data', pointsensor_token) im = Image.open(osp.join(nusc.dataroot, cam['filename'])) sample_rec = explorer.nusc.get('sample', pointsensor['sample_token']) chan = pointsensor['channel'] ref_chan = 'LIDAR_TOP' pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan, nsweeps=10) data_path, boxes, camera_intrinsic = nusc.get_sample_data( pointsensor_token, selected_anntokens=[box.token]) pcl_box = boxes[0] original_points = pc.points.copy() # 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. 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) center = np.array([[box.center[0]], [box.center[1]], [box.center[2]]]) box_center = view_points(center, np.array(cs_record['camera_intrinsic']), normalize=True) box_corners = box.corners() # Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons. # Also make sure points are at least 1m in front of the camera to avoid seeing the lidar points on the camera # casing for non-keyframes which are slightly out of sync. mask = np.ones(depths.shape[0], dtype=bool) mask = np.logical_and(mask, depths > min_dist) 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) original_points = original_points[:, mask] points = points[:, mask] points_2 = pc.points[:, mask] crop_mask = np.ones(points.shape[1], dtype=bool) crop_mask = np.logical_and(crop_mask, points[1, :] > bottom_left[1]) crop_mask = np.logical_and(crop_mask, points[1, :] < top_right[1]) crop_mask = np.logical_and(crop_mask, points[0, :] > bottom_left[0]) crop_mask = np.logical_and(crop_mask, points[0, :] < top_right[0]) original_points = original_points[:, crop_mask] points = points[:, crop_mask] points_3 = points_2[:, crop_mask] image_center = np.asarray([((bottom_left[0] + top_right[0]) / 2), ((bottom_left[1] + top_right[1]) / 2), 1]) # rotate to make the ray passing through the image_center into the z axis z_axis = np.linalg.lstsq(np.array(cs_record['camera_intrinsic']), image_center, rcond=None)[0] v = np.asarray([z_axis[0], z_axis[1], z_axis[2]]) z = np.asarray([0., 0., 1.]) normal = np.cross(v, z) theta = np.arccos(np.dot(v, z) / np.sqrt(v.dot(v))) new_pts = [] new_corner = [] old_pts = [] points_3 = points_3[:3, :] translate = np.dot(rotation_matrix(normal, theta), image_center) for point in points_3.T: new_pts = new_pts + [np.dot(rotation_matrix(normal, theta), point)] for corner in box_corners.T: new_corner = new_corner + [ np.dot(rotation_matrix(normal, theta), corner) ] points = np.asarray(new_pts) original_points = original_points[:3, :].T new_corners = np.asarray(new_corner) reverse_matrix = rotation_matrix(normal, -theta) # Sample 400 points if np.shape(new_pts)[0] > 400: mask = np.random.choice(points.shape[0], 400, replace=False) points = points[mask, :] original_points = original_points[mask, :] shift = np.expand_dims(np.mean(points, axis=0), 0) points = points - shift # center new_corners = new_corners - shift # center dist = np.max(np.sqrt(np.sum(points**2, axis=1)), 0) points = points / dist #scale new_corners = new_corners / dist #scale # Compute offset from point to corner n = np.shape(points)[0] offset = np.zeros((n, 8, 3)) for i in range(0, n): for j in range(0, 8): offset[i][j] = new_corners[j] - points[i] # Compute mask on which points lie inside of the box m = [] for point in original_points: if in_box(point, pcl_box.corners()) == True: m = m + [1] else: m = m + [0] m = np.asarray(m) return points.T, m, offset, reverse_matrix, new_corners
def visualize_sample(nusc: NuScenes, sample_token: str, all_annotations: Dict, all_results: Dict, nsweeps: int=1, conf_th: float=0.15, eval_range: float=40, verbose=True) -> None: """ Visualizes a sample from BEV with annotations and detection results. :param nusc: NuScenes object. :param sample_token: The nuScenes sample token. :param all_annotations: Maps each sample token to its annotations. :param all_results: Maps each sample token to its results. :param nsweeps: Number of sweeps used for lidar visualization. :param conf_th: The confidence threshold used to filter negatives. :param eval_range: Range in meters beyond which boxes are ignored. :param verbose: Whether to print to stdout. """ # Retrieve sensor & pose records. sample_rec = nusc.get('sample', sample_token) sd_record = nusc.get('sample_data', sample_rec['data']['LIDAR_TOP']) cs_record = nusc.get('calibrated_sensor', sd_record['calibrated_sensor_token']) pose_record = nusc.get('ego_pose', sd_record['ego_pose_token']) # Get boxes. boxes_gt_global = all_annotations[sample_token] boxes_est_global = all_results[sample_token] # Map GT boxes to lidar. boxes_gt = boxes_to_sensor(boxes_gt_global, pose_record, cs_record) # Map EST boxes to lidar. boxes_est = boxes_to_sensor(boxes_est_global, pose_record, cs_record) # Get point cloud in lidar frame. pc, _ = LidarPointCloud.from_file_multisweep(nusc, sample_rec, 'LIDAR_TOP', 'LIDAR_TOP', nsweeps=nsweeps) # Init axes. _, 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 / eval_range) ax.scatter(points[0, :], points[1, :], c=colors, s=0.2) # Show ego vehicle. ax.plot(0, 0, 'x', color='black') # Show GT boxes. for box in boxes_gt: box.render(ax, view=np.eye(4), colors=('g', 'g', 'g'), linewidth=2) # Show EST boxes. for box in boxes_est: # Show only predictions with a high score. if box.score >= conf_th: box.render(ax, view=np.eye(4), colors=('b', 'b', 'b'), linewidth=1) # Limit visible range. axes_limit = eval_range + 3 # Slightly bigger to include boxes that extend beyond the range. ax.set_xlim(-axes_limit, axes_limit) ax.set_ylim(-axes_limit, axes_limit) # Show plot. if verbose: print('Showing sample token %s' % sample_token) plt.title(sample_token) plt.show()
def create_dataset(dataroot, save_dir, width=672, height=672, grid_range=70., nusc_version='v1.0-mini', use_constant_feature=True, use_intensity_feature=True, end_id=None): os.makedirs(os.path.join(save_dir, 'in_feature'), exist_ok=True) os.makedirs(os.path.join(save_dir, 'out_feature'), exist_ok=True) nusc = NuScenes(version=nusc_version, dataroot=dataroot, verbose=True) ref_chan = 'LIDAR_TOP' if width == height: size = width else: raise Exception( 'Currently only supported if width and height are equal') grid_length = 2. * grid_range / size label_half_length = 0 data_id = 0 grid_ticks = np.arange(-grid_range, grid_range + grid_length, grid_length) grid_centers \ = (grid_ticks + grid_length / 2)[:len(grid_ticks) - 1] for my_scene in nusc.scene: first_sample_token = my_scene['first_sample_token'] token = first_sample_token # try: while (token != ''): print('--- {} '.format(data_id) + token + ' ---') my_sample = nusc.get('sample', token) sd_record = nusc.get('sample_data', my_sample['data'][ref_chan]) sample_rec = nusc.get('sample', sd_record['sample_token']) chan = sd_record['channel'] pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan, nsweeps=1) _, boxes, _ = nusc.get_sample_data(sd_record['token'], box_vis_level=0) out_feature = np.zeros((size, size, 8), dtype=np.float32) start = time.time() for box_idx, box in enumerate(boxes): label = 0 if box.name.split('.')[0] == 'vehicle': if box.name.split('.')[1] == 'car': label = 1 elif box.name.split('.')[1] == 'bus': label = 2 elif box.name.split('.')[1] == 'truck': label = 2 elif box.name.split('.')[1] == 'construction': label = 2 elif box.name.split('.')[1] == 'emergency': label = 2 elif box.name.split('.')[1] == 'trailer': label = 2 elif box.name.split('.')[1] == 'bicycle': label = 3 elif box.name.split('.')[1] == 'motorcycle': label = 3 elif box.name.split('.')[0] == 'human': label = 4 # elif box.name.split('.')[0] == 'movable_object': # label = 1 # elif box.name.split('.')[0] == 'static_object': # label = 1 else: continue height_pt = np.linalg.norm(box.corners().T[0] - box.corners().T[3]) box_corners = box.corners().astype(np.float32) corners2d = box_corners[:2, :] # corners2d = box.corners()[:2, :].astype(np.float32) box2d = corners2d.T[[2, 3, 7, 6]] box2d_center = box2d.mean(axis=0) yaw, pitch, roll = box.orientation.yaw_pitch_roll # print('--') # print(box.name) p1_reshape = box_corners out_feature = generate_out_feature( width, height, size, grid_centers, box_corners, box2d, box2d_center, pc.points.astype(np.float32), height_pt, label, label_half_length, yaw, out_feature) # generate_out_feature(width, height, size, grid_centers, pc.points, # box.corners(), height_pt, # label, label_half_length, yaw, # out_feature) # out_feature = out_feature.astype(np.float32) out_end = time.time() # feature_generator = fg.FeatureGenerator( # grid_range, width, height, # use_constant_feature, use_intensity_feature) feature_generator = fgpb.FeatureGenerator(grid_range, width, height) in_feature = feature_generator.generate(pc.points.T, use_constant_feature, use_intensity_feature) in_end = time.time() print('time total {} out {} in {}'.format(in_end - start, out_end - start, in_end - out_end)) if use_constant_feature and use_intensity_feature: channels = 8 elif use_constant_feature or use_intensity_feature: channels = 6 else: channels = 4 in_feature = np.array(in_feature).reshape(channels, size, size).astype(np.float16) in_feature = in_feature.transpose(1, 2, 0) # instance_pt is flipped due to flip # out_feature = np.flip(np.flip(out_feature, axis=0), axis=1) # out_feature[:, :, 1:3] *= -1 np.save(os.path.join(save_dir, 'in_feature/{:05}'.format(data_id)), in_feature) np.save( os.path.join(save_dir, 'out_feature/{:05}'.format(data_id)), out_feature) token = my_sample['next'] data_id += 1 if data_id == end_id: return
def create_dataset(dataroot, save_dir, width=672, height=672, grid_range=70., nusc_version='v1.0-mini', use_constant_feature=True, use_intensity_feature=True, end_id=None): os.makedirs(os.path.join(save_dir, 'in_feature'), exist_ok=True) os.makedirs(os.path.join(save_dir, 'out_feature'), exist_ok=True) nusc = NuScenes(version=nusc_version, dataroot=dataroot, verbose=True) ref_chan = 'LIDAR_TOP' if width == height: size = width else: raise Exception( 'Currently only supported if width and height are equal') grid_length = 2. * grid_range / size label_half_length = 10 data_id = 0 grid_ticks = np.arange(-grid_range, grid_range + grid_length, grid_length) grid_centers \ = (grid_ticks + grid_length / 2)[:len(grid_ticks) - 1] for my_scene in nusc.scene: first_sample_token = my_scene['first_sample_token'] token = first_sample_token while (token != ''): print('--- {} '.format(data_id) + token + ' ---') my_sample = nusc.get('sample', token) sd_record = nusc.get('sample_data', my_sample['data'][ref_chan]) sample_rec = nusc.get('sample', sd_record['sample_token']) chan = sd_record['channel'] pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan, nsweeps=10) _, boxes, _ = nusc.get_sample_data(sd_record['token'], box_vis_level=0) out_feature = np.zeros((size, size, 7), dtype=np.float32) for box_idx, box in enumerate(boxes): label = 0 if box.name.split('.')[0] == 'vehicle': if box.name.split('.')[1] == 'car': label = 1 elif (box.name.split('.')[1] in ['bus', 'construction', 'trailer', 'truck']): label = 2 elif box.name.split('.')[1] == 'emergency': if box.name.split('.')[2] == 'ambulance': label = 2 # police else: label = 1 elif box.name.split('.')[1] in ['bicycle', 'motorcycle']: label = 3 elif box.name.split('.')[0] == 'human': label = 4 elif (box.name.split('.')[0] in ['animal', 'movable_object', 'static_object']): label = 5 else: continue height_pt = np.linalg.norm(box.corners().T[0] - box.corners().T[3]) corners2d = box.corners()[:2, :] box2d = corners2d.T[[2, 3, 7, 6]] box2d_center = box2d.mean(axis=0) generate_out_feature(width, height, size, grid_centers, box2d, box2d_center, height_pt, label, label_half_length, out_feature) out_feature = out_feature.astype(np.float16) feature_generator = fg.Feature_generator(grid_range, width, height, use_constant_feature, use_intensity_feature) feature_generator.generate(pc.points.T) in_feature = feature_generator.feature if use_constant_feature and use_intensity_feature: in_feature = in_feature.reshape(size, size, 8) elif use_constant_feature or use_intensity_feature: in_feature = in_feature.reshape(size, size, 6) else: in_feature = in_feature.reshape(size, size, 4) # instance_pt is flipped due to flip out_feature = np.flip(np.flip(out_feature, axis=0), axis=1) out_feature[:, :, 1:3] *= -1 np.save(os.path.join(save_dir, 'in_feature/{:05}'.format(data_id)), in_feature) np.save( os.path.join(save_dir, 'out_feature/{:05}'.format(data_id)), out_feature) token = my_sample['next'] data_id += 1 if data_id == end_id: return
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()