def filter_points_in_ann(self, ann_token: str, wlh_factor: int = 3): points_xyz = np.vstack( (self.points[:2, :], np.zeros(self.nbr_points()))) mask = points_in_box(box=self.box(ann_token), points=points_xyz, wlh_factor=wlh_factor) self.points = self.points[:, mask] # np.shape(pc_cs.points) = 6 x N
def test_points_in_box(self): """ Test the box.in_box method. """ vel = (np.nan, np.nan, np.nan) def qyaw(yaw): return Quaternion(axis=(0, 0, 1), angle=yaw) # Check points inside box box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel) points = np.array([[0.0, 0.0, 0.0], [0.5, 0.5, 0.0]]).transpose() mask = points_in_box(box, points, wlh_factor=1.0) self.assertEqual(mask.all(), True) # Check points outside box box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel) points = np.array([[0.1, 0.0, 0.0], [0.5, -1.1, 0.0]]).transpose() mask = points_in_box(box, points, wlh_factor=1.0) self.assertEqual(mask.all(), False) # Check corner cases box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel) points = np.array([[-1.0, -1.0, 0.0], [1.0, 1.0, 0.0]]).transpose() mask = points_in_box(box, points, wlh_factor=1.0) self.assertEqual(mask.all(), True) # Check rotation (45 degs) and translation (by [1,1]) rot = 45 trans = [1.0, 1.0] box = Box([0.0 + trans[0], 0.0 + trans[1], 0.0], [2.0, 2.0, 0.0], qyaw(rot / 180.0 * np.pi), 1, 2.0, vel) points = np.array([[0.70 + trans[0], 0.70 + trans[1], 0.0], [0.71 + 1.0, 0.71 + 1.0, 0.0]]).transpose() mask = points_in_box(box, points, wlh_factor=1.0) self.assertEqual(mask[0], True) self.assertEqual(mask[1], False) # Check 3d box box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 2.0], qyaw(0.0), 1, 2.0, vel) points = np.array([[0.0, 0.0, 0.0], [0.5, 0.5, 0.5]]).transpose() mask = points_in_box(box, points, wlh_factor=1.0) self.assertEqual(mask.all(), True) # Check wlh factor for wlh_factor in [0.5, 1.0, 1.5, 10.0]: box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel) points = np.array([[0.0, 0.0, 0.0], [0.5, 0.5, 0.0]]).transpose() mask = points_in_box(box, points, wlh_factor=wlh_factor) self.assertEqual(mask.all(), True) for wlh_factor in [0.1, 0.49]: box = Box([0.0, 0.0, 0.0], [2.0, 2.0, 0.0], qyaw(0.0), 1, 2.0, vel) points = np.array([[0.0, 0.0, 0.0], [0.5, 0.5, 0.0]]).transpose() mask = points_in_box(box, points, wlh_factor=wlh_factor) self.assertEqual(mask[0], True) self.assertEqual(mask[1], False)
def filter_eval_boxes(nusc: NuScenes, eval_boxes: EvalBoxes, max_dist: Dict[str, float], verbose: bool = False) -> EvalBoxes: """ Applies filtering to boxes. Distance, bike-racks and points per box. :param nusc: An instance of the NuScenes class. :param eval_boxes: An instance of the EvalBoxes class. :param max_dist: Maps the detection name to the eval distance threshold for that class. :param verbose: Whether to print to stdout. """ # Accumulators for number of filtered boxes. total, dist_filter, point_filter, bike_rack_filter = 0, 0, 0, 0 for ind, sample_token in enumerate(eval_boxes.sample_tokens): # Filter on distance first total += len(eval_boxes[sample_token]) eval_boxes.boxes[sample_token] = [box for box in eval_boxes[sample_token] if box.ego_dist < max_dist[box.detection_name]] dist_filter += len(eval_boxes[sample_token]) # Then remove boxes with zero points in them. Eval boxes have -1 points by default. eval_boxes.boxes[sample_token] = [box for box in eval_boxes[sample_token] if not box.num_pts == 0] point_filter += len(eval_boxes[sample_token]) # Perform bike-rack filtering sample_anns = nusc.get('sample', sample_token)['anns'] bikerack_recs = [nusc.get('sample_annotation', ann) for ann in sample_anns if nusc.get('sample_annotation', ann)['category_name'] == 'static_object.bicycle_rack'] bikerack_boxes = [Box(rec['translation'], rec['size'], Quaternion(rec['rotation'])) for rec in bikerack_recs] filtered_boxes = [] for box in eval_boxes[sample_token]: if box.detection_name in ['bicycle', 'motorcycle']: in_a_bikerack = False for bikerack_box in bikerack_boxes: if np.sum(points_in_box(bikerack_box, np.expand_dims(np.array(box.translation), axis=1))) > 0: in_a_bikerack = True if not in_a_bikerack: filtered_boxes.append(box) else: filtered_boxes.append(box) eval_boxes.boxes[sample_token] = filtered_boxes bike_rack_filter += len(eval_boxes.boxes[sample_token]) if verbose: print("=> Original number of boxes: %d" % total) print("=> After distance based filtering: %d" % dist_filter) print("=> After LIDAR points based filtering: %d" % point_filter) print("=> After bike rack filtering: %d" % bike_rack_filter) return eval_boxes
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 render(self, events: DataFrame, timestamp: int, frame_gt: List[TrackingBox], frame_pred: List[TrackingBox], points=None, pose_record=None, cs_record=None, ifplotgt=False,\ threshold=0.1, ifpltsco=False, outscen_class=False,nusc=None, ifplthis=False, pltve=False) \ -> None: """ Render function for a given scene timestamp :param events: motmetrics events for that particular :param timestamp: timestamp for the rendering :param frame_gt: list of ground truth boxes :param frame_pred: list of prediction boxes """ # Init. #print('Rendering {}'.format(timestamp)) switches = events[events.Type == 'SWITCH'] switch_ids = switches.HId.values #对应frame_pred的tracking_id switch_gt_ids = switches.OId.values #对应GT的tracking_id FN = events[events.Type == 'MISS'] #FN = FN.HId.values FN = FN.OId.values #对应GT的tracking_id FP = events[events.Type == 'FP'] FP = FP.HId.values #对应frame_pred的tracking_id fig, ax = plt.subplots() #plt.style.use('dark_background') # 黑 背景颜色 plt.style.use('classic') # 白 背景颜色 points.render_height(ax, view=np.eye(4), x_lim=(-50, 50), y_lim=(-50, 50), marker_size=0.1) #BEV #points.render_height(ax, view=np.eye(4) )#BEV #points = points.rotate(Quaternion( pose_record["rotation"]).inverse) sam_anno = [] if len(frame_gt) > 0: sample = nusc.get('sample', frame_gt[0].sample_token) sample_annotation_tokens = sample['anns'] #标注 for anno_token in sample_annotation_tokens: sam_anno.append( nusc.get('sample_annotation', anno_token)["instance_token"]) vislev = {'v0-40': 0, 'v40-60': 1, 'v60-80': 2, 'v80-100': 3} #points.render_intensity(ax) # Plot GT boxes. if ifplotgt: for i, b in enumerate(frame_gt): color = 'k' #qua = tuple(self.list_add(list(b.rotation),[0.924,0.0,0.0,0.383])) box = Box(np.array(b.ego_translation, dtype=float), b.size, Quaternion(b.rotation), name=b.tracking_name, token=b.tracking_id) #qua = tuple(self.list_add(list(pose_record["rotation"]),[ 0.831,0.0,0.0,0.556])) #box.translate(-np.array(pose_record["translation"])) box.rotate(Quaternion(pose_record["rotation"]).inverse) # move box to sensor coord system box.translate(-np.array(cs_record["translation"])) box.rotate(Quaternion(cs_record["rotation"]).inverse) if outscen_class: #####TrackingRenderer.gt_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-50nums':0, '50-200nums':0, '>200nums': 0 } #car lidar点云数 #####TrackingRenderer.gt_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-20nums':0, '20-30nums':0, '>30nums': 0 } #ped lidar点云数 num_pts = frame_gt[i].num_pts #####car if TrackingRenderer.outscen == 'car': if num_pts > 0 and num_pts <= 5: TrackingRenderer.gt_ptsnumrange['0-5nums'] += 1 elif num_pts > 5 and num_pts <= 10: TrackingRenderer.gt_ptsnumrange['5-10nums'] += 1 elif num_pts > 10 and num_pts <= 50: TrackingRenderer.gt_ptsnumrange['10-50nums'] += 1 elif num_pts > 50 and num_pts <= 200: TrackingRenderer.gt_ptsnumrange['50-200nums'] += 1 elif num_pts > 200: TrackingRenderer.gt_ptsnumrange['>200nums'] += 1 else: ####ped if num_pts > 0 and num_pts <= 5: TrackingRenderer.gt_ptsnumrange['0-5nums'] += 1 elif num_pts > 5 and num_pts <= 10: TrackingRenderer.gt_ptsnumrange['5-10nums'] += 1 elif num_pts > 10 and num_pts <= 20: TrackingRenderer.gt_ptsnumrange['10-20nums'] += 1 elif num_pts > 20 and num_pts <= 30: TrackingRenderer.gt_ptsnumrange['20-30nums'] += 1 elif num_pts > 30: TrackingRenderer.gt_ptsnumrange['>30nums'] += 1 if box.token in FN: #####distance dis = math.sqrt(box.center[0]**2 + box.center[1]**2) if dis > 0 and dis <= 15: TrackingRenderer.fn_disrange[ '<15m'] = TrackingRenderer.fn_disrange[ '<15m'] + 1 elif dis > 15 and dis <= 30: TrackingRenderer.fn_disrange[ '15-30m'] = TrackingRenderer.fn_disrange[ '15-30m'] + 1 elif dis > 30 and dis <= 45: TrackingRenderer.fn_disrange[ '30-45m'] = TrackingRenderer.fn_disrange[ '30-45m'] + 1 elif dis > 45 and dis <= 54: TrackingRenderer.fn_disrange[ '45-54m'] = TrackingRenderer.fn_disrange[ '45-54m'] + 1 else: TrackingRenderer.fn_disrange[ '-1'] = TrackingRenderer.fn_disrange['-1'] + 1 #####ve TrackingRenderer.fn_verange = {'0-0.1m/s':0, '0.1-2.5m/s':0, '2.5-5m/s':0, '5-10m/s':0, '>10m/s': 0 } #car 绝对速度 ##### TrackingRenderer.fn_verange = {'0-0.1m/s':0, '0.1-1.0m/s':0, '1.0-1.5m/s':0, '1.5-2m/s':0, '>2m/s': 0 } #ped 绝对速度 ve = math.sqrt(frame_gt[i].velocity[0]**2 + frame_gt[i].velocity[1]**2) if TrackingRenderer.outscen == 'car': if ve > 0 and ve <= 0.1: TrackingRenderer.fn_verange['0-0.1m/s'] += 1 elif ve > 0.1 and ve <= 2.5: TrackingRenderer.fn_verange['0.1-2.5m/s'] += 1 elif ve > 2.5 and ve <= 5: TrackingRenderer.fn_verange['2.5-5m/s'] += 1 elif ve > 5 and ve <= 10: TrackingRenderer.fn_verange['5-10m/s'] += 1 else: TrackingRenderer.fn_verange['>10m/s'] += 1 else: if ve > 0 and ve <= 0.1: TrackingRenderer.fn_verange['0-0.1m/s'] += 1 elif ve > 0.1 and ve <= 1.0: TrackingRenderer.fn_verange['0.1-1.0m/s'] += 1 elif ve > 1.0 and ve <= 1.5: TrackingRenderer.fn_verange['1.0-1.5m/s'] += 1 elif ve > 1.5 and ve <= 2: TrackingRenderer.fn_verange['1.5-2m/s'] += 1 else: TrackingRenderer.fn_verange['>2m/s'] += 1 #####num_pts TrackingRenderer.fn_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-50nums':0, '50-200nums':0, '>200nums': 0 } #car lidar点云数 抽样参考比例:0.21 0.23 0.26,0.2,0.1 #############TrackingRenderer.fn_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-20nums':0, '20-30nums':0, '>30nums': 0 } #ped lidar点云数 num_pts = frame_gt[i].num_pts if TrackingRenderer.outscen == 'car': if num_pts > 0 and num_pts <= 5: TrackingRenderer.fn_ptsnumrange['0-5nums'] += 1 elif num_pts > 5 and num_pts <= 10: TrackingRenderer.fn_ptsnumrange[ '5-10nums'] += 1 elif num_pts > 10 and num_pts <= 50: TrackingRenderer.fn_ptsnumrange[ '10-50nums'] += 1 elif num_pts > 50 and num_pts <= 200: TrackingRenderer.fn_ptsnumrange[ '50-200nums'] += 1 elif num_pts > 200: TrackingRenderer.fn_ptsnumrange[ '>200nums'] += 1 else: TrackingRenderer.fn_ptsnumrange['-1'] += 1 else: if num_pts > 0 and num_pts <= 5: TrackingRenderer.fn_ptsnumrange['0-5nums'] += 1 elif num_pts > 5 and num_pts <= 10: TrackingRenderer.fn_ptsnumrange[ '5-10nums'] += 1 elif num_pts > 10 and num_pts <= 20: TrackingRenderer.fn_ptsnumrange[ '10-20nums'] += 1 elif num_pts > 20 and num_pts <= 30: TrackingRenderer.fn_ptsnumrange[ '20-30nums'] += 1 elif num_pts > 200: TrackingRenderer.fn_ptsnumrange['>30nums'] += 1 else: TrackingRenderer.fn_ptsnumrange['-1'] += 1 ######读取 #sample = nusc.get('sample', frame_gt[i].sample_token) #sample_annotation_tokens = sample['anns'] #标注 try: ind = sam_anno.index(b.tracking_id) sample_annotation = nusc.get( 'sample_annotation', sample_annotation_tokens[ind]) ####TrackingRenderer.vis_ratio = {'0-0.4':0, '0.4-0.6':0, '0.6-0.8':0, '0.8-1.0':0} #相机视角 0-40%, 40-60%, 60-80% and 80-100% The visibility of an instance is the fraction of annotation visible in all 6 images. visibility = nusc.get( 'visibility', sample_annotation['visibility_token']) vis_level = vislev[visibility["level"]] if vis_level == 0: TrackingRenderer.vis_ratio['0-0.4'] += 1 elif vis_level == 1: TrackingRenderer.vis_ratio['0.4-0.6'] += 1 elif vis_level == 2: TrackingRenderer.vis_ratio['0.6-0.8'] += 1 elif vis_level == 3: TrackingRenderer.vis_ratio['0.8-1.0'] += 1 ####TrackingRenderer.gt_ratio = {'ang_muta':0, 've_muta':0, 'aug_other':0, 've_other':0, 'firfn_trk':0, 'nonfirfn_trk':0} #仅包含与上一帧持续追踪的样本,角度和速度突变分别与other相并 pre_token = sample_annotation['prev'] if pre_token == '': #仅作为first_FN TrackingRenderer.gt_ratio['firfn_trk'] += 1 else: TrackingRenderer.gt_ratio['nonfirfn_trk'] += 1 pre_annotation = nusc.get( 'sample_annotation', pre_token) vari_ang = abs( R.from_quat(list(frame_gt[i].rotation)). as_euler('zxy', degrees=False)[0] - R.from_quat( list(pre_annotation['rotation']) ).as_euler('zxy', degrees=False)[0]) if vari_ang > 0.52: #30度 TrackingRenderer.gt_ratio['angvar>30'] += 1 elif 0.52 > vari_ang and vari_ang > 0.35: TrackingRenderer.gt_ratio[ '30>angvar>20'] += 1 elif 0.35 > vari_ang and vari_ang > 0.17: TrackingRenderer.gt_ratio[ '20>angvar>10'] += 1 elif vari_ang < 0.17: TrackingRenderer.gt_ratio['10>angvar'] += 1 else: pass pre_ve = nusc.box_velocity( pre_annotation['token']) ve_varity = abs(ve - math.sqrt(pre_ve[0]**2 + pre_ve[1]**2)) if ve_varity > TrackingRenderer.mutave_thr[0]: TrackingRenderer.gt_ratio[ 'vevari>%s' % (TrackingRenderer.mutave_thr[0])] += 1 elif ve_varity < TrackingRenderer.mutave_thr[ 0] and ve_varity >= TrackingRenderer.mutave_thr[ 1]: TrackingRenderer.gt_ratio[ '%s<vevari<%s' % (TrackingRenderer.mutave_thr[1], TrackingRenderer.mutave_thr[0])] += 1 elif ve_varity < TrackingRenderer.mutave_thr[ 1] and ve_varity >= TrackingRenderer.mutave_thr[ 2]: TrackingRenderer.gt_ratio[ '%s<vevari<%s' % (TrackingRenderer.mutave_thr[2], TrackingRenderer.mutave_thr[1])] += 1 else: TrackingRenderer.gt_ratio[ 'vevari<%s' % TrackingRenderer.mutave_thr[2]] += 1 except ValueError: #标注错误 TrackingRenderer.fault_datas += 1 box.render(ax, view=np.eye(4), colors=(color, color, color), linewidth=1) else: pass # Plot predicted boxes. pred_trackid = [] for i, b in enumerate(frame_pred): box = Box( b.ego_translation, b.size, Quaternion(b.rotation), name=b.tracking_name, token=b.tracking_id, score=b.tracking_score, ) # move box to ego vehicle coord system before has done the translation box.rotate(Quaternion(pose_record["rotation"]).inverse) # move box to sensor coord system box.translate(-np.array(cs_record["translation"])) box.rotate(Quaternion(cs_record["rotation"]).inverse) pred_trackid.append(b.tracking_id) if outscen_class: if b.tracking_id in FP: #####distance TrackingRenderer.fp_disrange = {'<15m':0, '15-30m':0, '30-45m':0, '45-54m':0} dis = math.sqrt(box.center[0]**2 + box.center[1]**2) if dis > 0 and dis <= 15: TrackingRenderer.fp_disrange[ '<15m'] = TrackingRenderer.fp_disrange['<15m'] + 1 elif dis > 15 and dis <= 30: TrackingRenderer.fp_disrange[ '15-30m'] = TrackingRenderer.fp_disrange[ '15-30m'] + 1 elif dis > 30 and dis <= 45: TrackingRenderer.fp_disrange[ '30-45m'] = TrackingRenderer.fp_disrange[ '30-45m'] + 1 elif dis > 45 and dis <= 54: TrackingRenderer.fp_disrange[ '45-54m'] = TrackingRenderer.fp_disrange[ '45-54m'] + 1 else: TrackingRenderer.fp_disrange[ '-1'] = TrackingRenderer.fp_disrange['-1'] + 1 #####ve TrackingRenderer.fp_verange = {'0-0.1m/s':0, '0.1-2.5m/s':0, '2.5-5m/s':0, '5-10m/s':0, '>10m/s': 0 } #car 绝对速度 #####TrackingRenderer.fp_verange = {'0-0.1m/s':0, '0.1-1.0m/s':0, '1.0-1.5m/s':0, '1.5-2m/s':0, '>2m/s': 0 } #ped 绝对速度 ve = math.sqrt(frame_pred[i].velocity[0]**2 + frame_pred[i].velocity[1]**2) if TrackingRenderer.outscen == 'car': if ve > 0 and ve <= 0.1: TrackingRenderer.fp_verange['0-0.1m/s'] += 1 elif ve > 0.1 and ve <= 2.5: TrackingRenderer.fp_verange['0.1-2.5m/s'] += 1 elif ve > 2.5 and ve <= 5: TrackingRenderer.fp_verange['2.5-5m/s'] += 1 elif ve > 5 and ve <= 10: TrackingRenderer.fp_verange['5-10m/s'] += 1 else: TrackingRenderer.fp_verange['>10m/s'] += 1 else: if ve > 0 and ve <= 0.1: TrackingRenderer.fp_verange['0-0.1m/s'] += 1 elif ve > 0.1 and ve <= 1.0: TrackingRenderer.fp_verange['0.1-1.0m/s'] += 1 elif ve > 1.0 and ve <= 1.5: TrackingRenderer.fp_verange['1.0-1.5m/s'] += 1 elif ve > 1.5 and ve <= 2: TrackingRenderer.fp_verange['1.5-2m/s'] += 1 else: TrackingRenderer.fp_verange['>2m/s'] += 1 #####num_pts TrackingRenderer.fp_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-50nums':0, '50-200nums':0, '>200nums': 0 } #car lidar点云数 抽样参考比例:0.21 0.23 0.26,0.2,0.1 ########## TrackingRenderer.fp_ptsnumrange = {'0-5nums':0, '5-10nums':0, '10-20nums':0, '20-30nums':0, '>30nums': 0 } #ped lidar点云数 points_xyzr = np.stack(points.points, 1) points_xyz = points_xyzr[:, :3] points_mask = points_in_box(box, points.points[:3]) mask_indx = np.arange(points_mask.shape[0]) mask_indx = mask_indx[points_mask] num_pts = mask_indx.shape[0] if TrackingRenderer.outscen == 'car': if num_pts > 0 and num_pts <= 5: TrackingRenderer.fp_ptsnumrange['0-5nums'] += 1 elif num_pts > 5 and num_pts <= 10: TrackingRenderer.fp_ptsnumrange['5-10nums'] += 1 elif num_pts > 10 and num_pts <= 50: TrackingRenderer.fp_ptsnumrange['10-50nums'] += 1 elif num_pts > 50 and num_pts <= 200: TrackingRenderer.fp_ptsnumrange['50-200nums'] += 1 elif num_pts > 200: TrackingRenderer.fp_ptsnumrange['>200nums'] += 1 else: if num_pts > 0 and num_pts <= 5: TrackingRenderer.fp_ptsnumrange['0-5nums'] += 1 elif num_pts > 5 and num_pts <= 10: TrackingRenderer.fp_ptsnumrange['5-10nums'] += 1 elif num_pts > 10 and num_pts <= 20: TrackingRenderer.fp_ptsnumrange['10-20nums'] += 1 elif num_pts > 20 and num_pts <= 30: TrackingRenderer.fp_ptsnumrange['20-30nums'] += 1 elif num_pts > 30: TrackingRenderer.fp_ptsnumrange['>30nums'] += 1 #####TrackingRenderer.fpscorrange = {'0-0.1':0, '0.2-0.4':0, '0.4-0.6':0,'0.6-1.0':0} score = box.score if score >= 0 and score <= 0.1: TrackingRenderer.fpscorrange['0-0.1'] += 1 if score >= 0.2 and score <= 0.4: TrackingRenderer.fpscorrange['0.2-0.4'] += 1 if score >= 0.4 and score <= 0.6: TrackingRenderer.fpscorrange['0.4-0.6'] += 1 if score >= 0.6 and score <= 1.0: TrackingRenderer.fpscorrange['0.6-1.0'] += 1 #####TrackingRenderer.trk_ratio = {'ang_muta':0, 've_muta':0, 'aug_other':0, 've_other':0} #仅包含与上一帧持续追踪的样本,角度和速度突变分别与other相并 if box.token in TrackingRenderer.his_trackid: pre_box = TrackingRenderer.his_track[ TrackingRenderer.his_trackid.index(box.token)] vari_ang = abs( (R.from_quat(list(frame_pred[i].rotation)). as_euler('zxy', degrees=False)[0]) - (R.from_quat(list(pre_box.rotation)).as_euler( 'zxy', degrees=False)[0])) if vari_ang > 0.52: #30度 TrackingRenderer.trk_ratio['angvar>30'] += 1 elif 0.52 > vari_ang > 0.35: TrackingRenderer.trk_ratio['30>angvar>20'] += 1 elif 0.35 > vari_ang > 0.17: TrackingRenderer.trk_ratio['20>angvar>10'] += 1 elif vari_ang < 0.17: TrackingRenderer.trk_ratio['10>angvar'] += 1 pre_ve = pre_box.velocity ve = frame_pred[i].velocity ve_varity = abs( math.sqrt(ve[0]**2 + ve[1]**2) - math.sqrt(pre_ve[0]**2 + pre_ve[1]**2)) if ve_varity > TrackingRenderer.mutave_thr[ 0]: # car均匀加速度为2.778m/s^2 3*0.5s=1.5 TrackingRenderer.trk_ratio[ 'vevari>%s' % TrackingRenderer.mutave_thr[0]] += 1 elif ve_varity < TrackingRenderer.mutave_thr[ 0] and ve_varity >= TrackingRenderer.mutave_thr[ 1]: TrackingRenderer.trk_ratio[ '%s<vevari<%s' % (TrackingRenderer.mutave_thr[1], TrackingRenderer.mutave_thr[0])] += 1 elif ve_varity < TrackingRenderer.mutave_thr[ 1] and ve_varity >= TrackingRenderer.mutave_thr[ 2]: TrackingRenderer.trk_ratio[ '%s<vevari<%s' % (TrackingRenderer.mutave_thr[2], TrackingRenderer.mutave_thr[1])] += 1 else: TrackingRenderer.trk_ratio[ 'vevari<%s' % TrackingRenderer.mutave_thr[2]] += 1 # Determine color for this tracking id. if b.tracking_id not in self.id2color.keys(): self.id2color[b.tracking_id] = ( float(hash(b.tracking_id + 'r') % 256) / 255, float(hash(b.tracking_id + 'g') % 256) / 255, float(hash(b.tracking_id + 'b') % 256) / 255) if ifplthis: box_for_path = copy.deepcopy(box) box_for_path.rotate(Quaternion(cs_record["rotation"])) box_for_path.translate(np.array(cs_record["translation"])) box_for_path.rotate(Quaternion(pose_record["rotation"])) box_for_path.translate(np.array( pose_record["translation"])) #到全局 # 记录轨迹坐标 if b.tracking_id in self.track_path.keys(): self.track_path[b.tracking_id].append(box_for_path) else: self.track_path[b.tracking_id] = [box_for_path] # Render box. Highlight identity switches in red. if b.tracking_id in switch_ids: color = self.id2color[b.tracking_id] box.render(ax, view=np.eye(4), colors=('r', 'r', color)) if outscen_class: ###TrackingRenderer.ids_verange = {'0-0.1m/s':0, '0.1-2.5m/s':0, '2.5-5m/s':0, '5-10m/s':0, '>10m/s': 0 } #car 绝对速度 ###TrackingRenderer.ids_verange = {'0-0.1m/s':0, '0.1-1.0m/s':0, '1.0-1.5m/s':0, '1.5-2m/s':0, '>2m/s': 0 } #ped 绝对速度 ve = math.sqrt(frame_pred[i].velocity[0]**2 + frame_pred[i].velocity[1]**2) if TrackingRenderer.outscen == 'car': if ve > 0 and ve <= 0.1: TrackingRenderer.ids_verange['0-0.1m/s'] += 1 elif ve > 0.1 and ve <= 2.5: TrackingRenderer.ids_verange['0.1-2.5m/s'] += 1 elif ve > 2.5 and ve <= 5: TrackingRenderer.ids_verange['2.5-5m/s'] += 1 elif ve > 5 and ve <= 10: TrackingRenderer.ids_verange['5-10m/s'] += 1 else: TrackingRenderer.ids_verange['>10m/s'] += 1 else: if ve > 0 and ve <= 0.1: TrackingRenderer.ids_verange['0-0.1m/s'] += 1 elif ve > 0.1 and ve <= 1.0: TrackingRenderer.ids_verange['0.1-1.0m/s'] += 1 elif ve > 1.0 and ve <= 1.5: TrackingRenderer.ids_verange['1.0-1.5m/s'] += 1 elif ve > 1.5 and ve <= 2: TrackingRenderer.ids_verange['1.5-2m/s'] += 1 else: TrackingRenderer.ids_verange['>2m/s'] += 1 ####TrackingRenderer.ids_factratio = {'delay_trk':0, 'del_oth_trk':0, 'reappear':0, 'reapother':0, 've_muta':0, 've_other':0, 'reapdeltrk':0 } indx = np.where(switch_ids == b.tracking_id) gt_id = switch_gt_ids[indx] try: x = sam_anno.index(gt_id) #sample = nusc.get('sample', frame_gt[x].sample_token) #sample_annotation_tokens = sample['anns'] #标注 sample_annotation = nusc.get( 'sample_annotation', sample_annotation_tokens[x]) if sample_annotation['prev'] == '': #参考意义不大 TrackingRenderer.ids_factratio['del_oth_trk'] += 1 else: TrackingRenderer.ids_factratio['delay_trk'] += 1 visibility = nusc.get( 'visibility', sample_annotation['visibility_token']) vis_level = vislev[visibility["level"]] pre_annotation = nusc.get( "sample_annotation", sample_annotation['prev']) pre_vis = nusc.get( 'visibility', pre_annotation['visibility_token']) pre_vislevel = vislev[pre_vis["level"]] if vis_level > pre_vislevel or (vis_level == pre_vislevel and vis_level < 3): TrackingRenderer.ids_factratio['reappear'] += 1 elif vis_level == pre_vislevel: TrackingRenderer.ids_factratio[ 'reapdeltrk'] += 1 else: TrackingRenderer.ids_factratio[ 'reapother'] += 1 pre_ve = nusc.box_velocity(pre_annotation['token']) ve = nusc.box_velocity(sample_annotation['token']) ve_varity = abs( math.sqrt(ve[0]**2 + ve[1]**2) - math.sqrt(pre_ve[0]**2 + pre_ve[1]**2)) if ve_varity > TrackingRenderer.mutave_thr[ 0]: # car均匀加速度为2.778m/s^2 3*0.5s=1.5 TrackingRenderer.ids_factratio[ 'vevari>%s' % (TrackingRenderer.mutave_thr[0])] += 1 elif ve_varity < TrackingRenderer.mutave_thr[ 0] and ve_varity >= TrackingRenderer.mutave_thr[ 1]: TrackingRenderer.ids_factratio[ '%s<vevari<%s' % (TrackingRenderer.mutave_thr[1], TrackingRenderer.mutave_thr[0])] += 1 elif ve_varity < TrackingRenderer.mutave_thr[ 1] and ve_varity >= TrackingRenderer.mutave_thr[ 2]: TrackingRenderer.ids_factratio[ '%s<vevari<%s' % (TrackingRenderer.mutave_thr[2], TrackingRenderer.mutave_thr[1])] += 1 else: TrackingRenderer.ids_factratio[ 'vevari<%s' % TrackingRenderer.mutave_thr[2]] += 1 except: #标注错误 pass else: color = self.id2color[b.tracking_id] box.render(ax, view=np.eye(4), colors=(color, color, color)) # Render other infos if ifpltsco: corners = view_points(box.corners(), view=np.eye(4), normalize=False)[:2, :] # ax.text(4,5,"hhaa0.8", fontsize=5) # ax.text(4,5,"%.2f\n%.2f,%.2f"%(b.tracking_score,b.velocity[0],b.velocity[1]), fontsize=5) ax.text(box.center[0], box.center[1], "%.2f\nvx=%.2f,vy=%.2f" % (b.tracking_score, b.velocity[0], b.velocity[1]), fontdict={ 'size': '6', 'color': 'b' }) #ax.text(box.center[0],box.center[1],"%.2f\n%.2f,%.2f"%(b.tracking_score,b.velocity[0],b.velocity[1]), fontsize=5) #if pltve: #删去当前帧多余轨迹线 keys = list(self.track_path.keys()) for key in keys: if key not in pred_trackid: self.track_path.pop(key) # 画历史轨迹线: if ifplthis: for id in self.track_path.keys(): color = self.id2color[id] for box_path in self.track_path[id]: #转到当前帧局部 # move box to ego vehicle coord system before has done the translation box_path.translate(-np.array(pose_record["translation"])) box_path.rotate( Quaternion(pose_record["rotation"]).inverse) # move box to sensor coord system box_path.translate(-np.array(cs_record["translation"])) box_path.rotate(Quaternion(cs_record["rotation"]).inverse) ax.scatter(box_path.center[0], box_path.center[1], 10, color) #转回全局 box_path.rotate(Quaternion(cs_record["rotation"])) box_path.translate(np.array(cs_record["translation"])) box_path.rotate(Quaternion(pose_record["rotation"])) box_path.translate(np.array(pose_record["translation"])) TrackingRenderer.his_track = frame_pred TrackingRenderer.his_trackid = pred_trackid # Plot MOTA metrics. ly # 目标位置 左上角,距离 Y 轴 0.01 倍距离,距离 X 轴 0.95倍距离 self.cursumfp += len(FP) #当前场景累积值 self.cursumfn += len(FN) #print("FN=%d,FP=%d,switch_ids=%d,cursumfp=%d,cursumfn=%d" % ( len(FN), len(FP), len(switch_ids), self.cursumfp, self.cursumfn )) #ax.text(0.01, 0.95, "IDS:%d\nFP:%d\nFN:%d\ncur_sce sumfp:%d sumfn:%d\nthreshold:%f"%(len(switch_ids),len(FP),len(FN),self.cursumfp,self.cursumfn,threshold), transform=ax.transAxes, fontdict={'size': '10', 'color': 'b'}) # Plot ego pose. plt.scatter(0, 0, s=96, facecolors='none', edgecolors='k', marker='o') plt.xlim(-50, 50) plt.ylim(-50, 50) plt.axis('off') # Save to disk and close figure. fig.savefig(os.path.join(self.save_path, '{}.png'.format(timestamp))) plt.close(fig)
def preprocess(nusc, split_names, root_dir, out_dir, keyword=None, keyword_action=None, subset_name=None, location=None): # cannot process day/night and location at the same time assert not (bool(keyword) and bool(location)) if keyword: assert keyword_action in ['filter', 'exclude'] # init dict to save pkl_dict = {} for split_name in split_names: pkl_dict[split_name] = [] for i, sample in enumerate(nusc.sample): curr_scene_name = nusc.get('scene', sample['scene_token'])['name'] # get if the current scene is in train, val or test curr_split = None for split_name in split_names: if curr_scene_name in getattr(splits, split_name): curr_split = split_name break if curr_split is None: continue if subset_name == 'night': if curr_split == 'train': if curr_scene_name in splits.val_night: curr_split = 'val' if subset_name == 'singapore': if curr_split == 'train': if curr_scene_name in splits.val_singapore: curr_split = 'val' # filter for day/night if keyword: scene_description = nusc.get("scene", sample["scene_token"])["description"] if keyword.lower() in scene_description.lower(): if keyword_action == 'exclude': # skip sample continue else: if keyword_action == 'filter': # skip sample continue if location: scene = nusc.get("scene", sample["scene_token"]) if location not in nusc.get("log", scene['log_token'])['location']: continue lidar_token = sample["data"]["LIDAR_TOP"] cam_front_token = sample["data"]["CAM_FRONT"] lidar_path, boxes_lidar, _ = nusc.get_sample_data(lidar_token) cam_path, boxes_front_cam, cam_intrinsic = nusc.get_sample_data( cam_front_token) print('{}/{} {} {}'.format(i + 1, len(nusc.sample), curr_scene_name, lidar_path)) sd_rec_lidar = nusc.get('sample_data', sample['data']["LIDAR_TOP"]) cs_record_lidar = nusc.get('calibrated_sensor', sd_rec_lidar['calibrated_sensor_token']) pose_record_lidar = nusc.get('ego_pose', sd_rec_lidar['ego_pose_token']) sd_rec_cam = nusc.get('sample_data', sample['data']["CAM_FRONT"]) cs_record_cam = nusc.get('calibrated_sensor', sd_rec_cam['calibrated_sensor_token']) pose_record_cam = nusc.get('ego_pose', sd_rec_cam['ego_pose_token']) calib_infos = { "lidar2ego_translation": cs_record_lidar['translation'], "lidar2ego_rotation": cs_record_lidar['rotation'], "ego2global_translation_lidar": pose_record_lidar['translation'], "ego2global_rotation_lidar": pose_record_lidar['rotation'], "ego2global_translation_cam": pose_record_cam['translation'], "ego2global_rotation_cam": pose_record_cam['rotation'], "cam2ego_translation": cs_record_cam['translation'], "cam2ego_rotation": cs_record_cam['rotation'], "cam_intrinsic": cam_intrinsic, } # load lidar points pts = np.fromfile(lidar_path, dtype=np.float32, count=-1).reshape([-1, 5])[:, :3].T # map point cloud into front camera image pts_valid_flag, pts_cam_coord, pts_img = map_pointcloud_to_image( pts, (900, 1600, 3), calib_infos) # fliplr so that indexing is row, col and not col, row pts_img = np.ascontiguousarray(np.fliplr(pts_img)) # only use lidar points in the front camera image pts = pts[:, pts_valid_flag] num_pts = pts.shape[1] seg_labels = np.full(num_pts, fill_value=len(class_names_to_id), dtype=np.uint8) # only use boxes that are visible in camera valid_box_tokens = [box.token for box in boxes_front_cam] boxes = [box for box in boxes_lidar if box.token in valid_box_tokens] for box in boxes: # get points that lie inside of the box fg_mask = points_in_box(box, pts) det_class = category_to_detection_name(box.name) if det_class is not None: seg_labels[fg_mask] = class_names_to_id[det_class] # convert to relative path lidar_path = lidar_path.replace(root_dir + '/', '') cam_path = cam_path.replace(root_dir + '/', '') # transpose to yield shape (num_points, 3) pts = pts.T # append data to train, val or test list in pkl_dict data_dict = { 'points': pts, 'seg_labels': seg_labels, 'points_img': pts_img, # row, col format, shape: (num_points, 2) 'lidar_path': lidar_path, 'camera_path': cam_path, 'boxes': boxes_lidar, "sample_token": sample["token"], "scene_name": curr_scene_name, "calib": calib_infos } pkl_dict[curr_split].append(data_dict) # save to pickle file save_dir = osp.join(out_dir, 'preprocess') os.makedirs(save_dir, exist_ok=True) for split_name in split_names: save_path = osp.join( save_dir, '{}{}.pkl'.format(split_name, '_' + subset_name if subset_name else '')) with open(save_path, 'wb') as f: pickle.dump(pkl_dict[split_name], f) print('Wrote preprocessed data to ' + save_path)
nbr_samples = nusc.scene[scene_index]['nbr_samples'] first_sample_token = my_scene['first_sample_token'] curr_sample = nusc.get('sample', first_sample_token) prog = 0 for _ in range(nbr_samples): sample_tokens[prog] = curr_sample['token'] if curr_sample['next']: next_token = curr_sample['next'] curr_sample = nusc.get('sample', next_token) prog += 1 my_sample_token = sample_tokens[sample_index] my_sample = nusc.get('sample', my_sample_token) sensor_channel = "RADAR_FRONT" my_sample_data = nusc.get('sample_data', my_sample['data'][sensor_channel]) radar_data = get_sensor_sample_data(nusc, my_sample, sensor_channel) my_annotation_token = my_sample['anns'][18] #note that the annotation here is in globar coordinate symstem my_annotation_metadata = nusc.get('sample_annotation', my_annotation_token) #pay attention to the class Box in nuscenes-devkit #in get_sample_data will transform the coordinate from global to sensor frame _, boxes, _ = nusc.get_sample_data(my_sample_data['token']) box_list = [] for box in boxes: mask = points_in_box(box, radar_data[0:3, :]) #caculate the number of points contains in a bounding box num_of_pts = np.count_nonzero(mask) if num_of_pts != 0: box_list.append((box.name, num_of_pts)) print(box_list)
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 get_points_after_view_transform(points, box_t): wlh = box_t.wlh box_len = np.sqrt(wlh[0]**2 + wlh[1]**2) points_new = points[:, 0:3].transpose() mask = points_in_box(box_t, points_new, wlh_factor=1.0) if mask.sum() <= 1: global number_of_less number_of_less += 1 return None points_in = points_new.transpose()[mask] points_in = points_in - box_t.center points_in = points_in.transpose() ## 2. get normal vector of forwarding direction box_coors = box_t.corners().transpose() pq = box_coors[3] - box_coors[0] pr = box_coors[4] - box_coors[0] normal_1 = np.cross(pq, pr) ## 3. get the rotation angle between normal vector and base vector def unit_vector(vector): """ Returns the unit vector of the vector. """ return vector / np.linalg.norm(vector) def angle_between(v1, v2): """ Returns the angle in radians between vectors 'v1' and 'v2':: >>> angle_between((1, 0, 0), (0, 1, 0)) 1.5707963267948966 >>> angle_between((1, 0, 0), (1, 0, 0)) 0.0 >>> angle_between((1, 0, 0), (-1, 0, 0)) 3.141592653589793 """ v1_u = unit_vector(v1) v2_u = unit_vector(v2) return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) base_vector = np.array([0, 1, 0]) angle_t = angle_between(normal_1, base_vector) ### get the axis of rotation axis_t = np.cross(normal_1, base_vector) ## 4. get the matrix representation of view; angle --> Quaternion --> matrix representation quat = Quaternion._from_axis_angle(axis_t, angle_t) FLOAT_EPS = np.finfo(np.float).eps def _quat_to_matrix(quat): w, x, y, z = quat[0], quat[1], quat[2], quat[3] Nq = w * w + x * x + y * y + z * z if Nq < FLOAT_EPS: return np.eye(3) s = 2.0 / Nq X = x * s Y = y * s Z = z * s wX = w * X wY = w * Y wZ = w * Z xX = x * X xY = x * Y xZ = x * Z yY = y * Y yZ = y * Z zZ = z * Z return np.array([[1.0 - (yY + zZ), xY - wZ, xZ + wY], [xY + wZ, 1.0 - (xX + zZ), yZ - wX], [xZ - wY, yZ + wX, 1.0 - (xX + yY)]]) view_t = _quat_to_matrix(quat) ## 5. view transformation points_t = view_points(points_in, view_t, normalize=False) return points_t