def draw_scenes(points, gt_boxes=None, ref_boxes=None, ref_scores=None, ref_labels=None): if not isinstance(points, np.ndarray): points = points.cpu().numpy() if ref_boxes is not None and not isinstance(ref_boxes, np.ndarray): ref_boxes = ref_boxes.cpu().numpy() if gt_boxes is not None and not isinstance(gt_boxes, np.ndarray): gt_boxes = gt_boxes.cpu().numpy() if ref_scores is not None and not isinstance(ref_scores, np.ndarray): ref_scores = ref_scores.cpu().numpy() if ref_labels is not None and not isinstance(ref_labels, np.ndarray): ref_labels = ref_labels.cpu().numpy() fig = visualize_pts(points) fig = draw_multi_grid_range(fig, bv_range=(0, -40, 80, 40)) if gt_boxes is not None: corners3d = box_utils.boxes_to_corners_3d(gt_boxes) fig = draw_corners3d(corners3d, fig=fig, color=(0, 0, 1), max_num=100) if ref_boxes is not None: ref_corners3d = box_utils.boxes_to_corners_3d(ref_boxes) if ref_labels is None: fig = draw_corners3d(ref_corners3d, fig=fig, color=(0, 1, 0), cls=ref_scores, max_num=100) else: for k in range(ref_labels.min(), ref_labels.max() + 1): cur_color = tuple(box_colormap[k % len(box_colormap)]) mask = (ref_labels == k) fig = draw_corners3d(ref_corners3d[mask], fig=fig, color=cur_color, cls=ref_scores[mask], max_num=100) mlab.view(azimuth=-179, elevation=54.0, distance=104.0, roll=90.0) return fig
def viz(self, bbox3d, frame_id): marker_array = MarkerArray() marker = Marker() marker.header.frame_id = frame_id marker.type = marker.LINE_LIST marker.action = marker.ADD marker.header.stamp = rospy.Time.now() # marker scale (scale y and z not used due to being linelist) marker.scale.x = 0.08 # marker color marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.points = [] corner_for_box_list = [ 0, 1, 0, 3, 2, 3, 2, 1, 4, 5, 4, 7, 6, 7, 6, 5, 3, 7, 0, 4, 1, 5, 2, 6 ] corners3d = boxes_to_corners_3d(bbox3d) # (N,8,3) for box_nr in range(corners3d.shape[0]): box3d_pts_3d_velo = corners3d[box_nr] # (8,3) for corner in corner_for_box_list: transformed_p = np.array(box3d_pts_3d_velo[corner, 0:4]) # transformed_p = transform_point(p, np.linalg.inv(self.Tr_velo_kitti_cam)) p = Point() p.x = transformed_p[0] p.y = transformed_p[1] p.z = transformed_p[2] marker.points.append(p) marker_array.markers.append(marker) id = 0 for m in marker_array.markers: m.id = id id += 1 self.mk_pub.publish(marker_array) marker_array.markers = [] pass
def forward(self, data_dict): spatial_features_2d = data_dict['spatial_features_2d'] cls_preds = self.conv_cls(spatial_features_2d) box_preds = self.conv_box(spatial_features_2d) cls_preds = cls_preds.permute(0, 2, 3, 1).contiguous() # [N, H, W, C] box_preds = box_preds.permute(0, 2, 3, 1).contiguous() # [N, H, W, C] self.forward_ret_dict['cls_preds'] = cls_preds self.forward_ret_dict['box_preds'] = box_preds if self.conv_dir_cls is not None: dir_cls_preds = self.conv_dir_cls(spatial_features_2d) dir_cls_preds = dir_cls_preds.permute(0, 2, 3, 1).contiguous() self.forward_ret_dict['dir_cls_preds'] = dir_cls_preds else: dir_cls_preds = None if self.training: gt_boxes_enlarged = None if self.model_cfg.get('USE_MULTIFRAME_ENLARGED_GT_BOXES', False): from pcdet.utils.box_utils import boxes_to_corners_3d from pcdet.utils.common_utils import rotate_points_along_z batch_size, num_boxes, boxes_dim = data_dict['gt_boxes'].shape gt_boxes_enlarged = data_dict['gt_boxes'].clone() if num_boxes > 0: stack_frame_size = data_dict['locations'].shape[2] gt_boxes_corner = [] locations = data_dict['locations'].view( -1, stack_frame_size, 3) rotations_y = data_dict['rotations_y'].view( -1, stack_frame_size) gt_boxes = data_dict['gt_boxes'].view(-1, boxes_dim) cur_gt_boxes = gt_boxes.clone() for idx in range(stack_frame_size): cur_gt_boxes[:, 0:3] = locations[:, idx, :] cur_gt_boxes[:, -1] = rotations_y[:, idx] gt_boxes_corner.append( boxes_to_corners_3d(cur_gt_boxes)) gt_boxes_corner = torch.cat(gt_boxes_corner, dim=1) gt_boxes_corner -= gt_boxes[:, None, 0:3] gt_boxes_corner_local = rotate_points_along_z( gt_boxes_corner, -gt_boxes[:, -2]) multi_length = gt_boxes_corner_local[:, :, 0].max( dim=1)[0] - gt_boxes_corner_local[:, :, 0].min(dim=1)[0] multi_width = gt_boxes_corner_local[:, :, 1].max( dim=1)[0] - gt_boxes_corner_local[:, :, 1].min(dim=1)[0] gt_boxes_enlarged = torch.cat([ gt_boxes[:, 0:3], multi_length[:, None], multi_width[:, None], gt_boxes[:, 5:] ], dim=-1) gt_boxes_enlarged = gt_boxes_enlarged.view( batch_size, num_boxes, boxes_dim) data_dict['gt_boxes_enlarged'] = gt_boxes_enlarged targets_dict = self.assign_targets( gt_boxes=data_dict['gt_boxes'], gt_boxes_enlarged=gt_boxes_enlarged) self.forward_ret_dict.update(targets_dict) if not self.training or self.predict_boxes_when_training: batch_cls_preds, batch_box_preds = self.generate_predicted_boxes( batch_size=data_dict['batch_size'], cls_preds=cls_preds, box_preds=box_preds, dir_cls_preds=dir_cls_preds) data_dict['batch_cls_preds'] = batch_cls_preds data_dict['batch_box_preds'] = batch_box_preds data_dict['cls_preds_normalized'] = False return data_dict
def process_single_scene(sample_idx): print('%s sample_idx: %s' % (self.split, sample_idx)) info = {} pc_info = {'num_features': 4, 'lidar_idx': sample_idx} info['point_cloud'] = pc_info image_info = {'image_idx': sample_idx, 'image_shape': self.get_image_shape(sample_idx)} info['image'] = image_info calib = self.get_calib(sample_idx) P2 = np.concatenate([calib.P2, np.array([[0., 0., 0., 1.]])], axis=0) R0_4x4 = np.zeros([4, 4], dtype=calib.R0.dtype) R0_4x4[3, 3] = 1. R0_4x4[:3, :3] = calib.R0 V2C_4x4 = np.concatenate([calib.V2C, np.array([[0., 0., 0., 1.]])], axis=0) calib_info = {'P2': P2, 'R0_rect': R0_4x4, 'Tr_velo_to_cam': V2C_4x4} info['calib'] = calib_info if has_label: obj_list = self.get_label(sample_idx) annotations = {} annotations['name'] = np.array([obj.cls_type for obj in obj_list]) annotations['truncated'] = np.array([obj.truncation for obj in obj_list]) annotations['occluded'] = np.array([obj.occlusion for obj in obj_list]) annotations['alpha'] = np.array([obj.alpha for obj in obj_list]) annotations['bbox'] = np.concatenate([obj.box2d.reshape(1, 4) for obj in obj_list], axis=0) annotations['dimensions'] = np.array([[obj.l, obj.h, obj.w] for obj in obj_list]) # lhw(camera) format annotations['location'] = np.concatenate([obj.loc.reshape(1, 3) for obj in obj_list], axis=0) annotations['rotation_y'] = np.array([obj.ry for obj in obj_list]) annotations['score'] = np.array([obj.score for obj in obj_list]) annotations['difficulty'] = np.array([obj.level for obj in obj_list], np.int32) num_objects = len([obj.cls_type for obj in obj_list if obj.cls_type != 'DontCare']) num_gt = len(annotations['name']) index = list(range(num_objects)) + [-1] * (num_gt - num_objects) annotations['index'] = np.array(index, dtype=np.int32) loc = annotations['location'][:num_objects] #kitti的label是当前帧的don't care都在最后 dims = annotations['dimensions'][:num_objects] rots = annotations['rotation_y'][:num_objects] loc_lidar = calib.rect_to_lidar(loc) l, h, w = dims[:, 0:1], dims[:, 1:2], dims[:, 2:3] # 转到标准坐标系下 # 1、应该是原kitti的高是box底部的高,这里转到了box中心。。同样,后面预测的结果还会再转回来:xyz_lidar[:, 2] -= h.reshape(-1) / 2 loc_lidar[:, 2] += h[:, 0] / 2 # 2、朝向角,后面转回来:r = -r - np.pi / 2 gt_boxes_lidar = np.concatenate([loc_lidar, l, w, h, -(np.pi / 2 + rots[..., np.newaxis])], axis=1) annotations['gt_boxes_lidar'] = gt_boxes_lidar info['annos'] = annotations if count_inside_pts: points = self.get_lidar(sample_idx) calib = self.get_calib(sample_idx) pts_rect = calib.lidar_to_rect(points[:, 0:3]) fov_flag = self.get_fov_flag(pts_rect, info['image']['image_shape'], calib) pts_fov = points[fov_flag] corners_lidar = box_utils.boxes_to_corners_3d(gt_boxes_lidar) num_points_in_gt = -np.ones(num_gt, dtype=np.int32) for k in range(num_objects): flag = box_utils.in_hull(pts_fov[:, 0:3], corners_lidar[k]) num_points_in_gt[k] = flag.sum() annotations['num_points_in_gt'] = num_points_in_gt return info