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