def on_scan(scan):
start = timeit.default_timer()
rospy.loginfo("Got scan")
gen = []
for p in pc2.read_points(scan, field_names = ("x", "y", "z", "intensity"), skip_nans=True):
gen.append(np.array([p[0], p[1], p[2], p[3]/100.0]))
gen_numpy = np.array(gen, dtype=np.float32)
front_lidar = get_filtered_lidar(gen_numpy, cnf.boundary)
bev_map = makeBEVMap(front_lidar, cnf.boundary)
bev_map = torch.from_numpy(bev_map)
with torch.no_grad():
detections, bev_map, fps = do_detect(configs, model, bev_map, is_front=True)
print(fps)
objects_msg = DetectedObjectArray()
objects_msg.header.stamp = rospy.Time.now()
objects_msg.header.frame_id = scan.header.frame_id
flag = False
for j in range(configs.num_classes):
class_name = ID_TO_CLASS_NAME[j]
if len(detections[j]) > 0:
flag = True
for det in detections[j]:
_score, _x, _y, _z, _h, _w, _l, _yaw = det
yaw = -_yaw
x = _y / cnf.BEV_HEIGHT * cnf.bound_size_x + cnf.boundary['minX']
y = _x / cnf.BEV_WIDTH * cnf.bound_size_y + cnf.boundary['minY']
z = _z + cnf.boundary['minZ']
w = _w / cnf.BEV_WIDTH * cnf.bound_size_y
l = _l / cnf.BEV_HEIGHT * cnf.bound_size_x
obj = DetectedObject()
obj.header.stamp = rospy.Time.now()
obj.header.frame_id = scan.header.frame_id
obj.score = 0.9
obj.pose_reliable = True
obj.space_frame = scan.header.frame_id
obj.label = class_name
obj.score = _score
obj.pose.position.x = x
obj.pose.position.y = y
obj.pose.position.z = z
[qx, qy, qz, qw] = euler_to_quaternion(yaw, 0, 0)
obj.pose.orientation.x = qx
obj.pose.orientation.y = qy
obj.pose.orientation.z = qz
obj.pose.orientation.w = qw
obj.dimensions.x = l
obj.dimensions.y = w
obj.dimensions.z = _h
objects_msg.objects.append(obj)
if flag is True:
pub.publish(objects_msg)
stop = timeit.default_timer()
print('Time: ', stop - start)
model = create_model(configs)
print('\n\n' + '-*=' * 30 + '\n\n')
assert os.path.isfile(configs.pretrained_path), "No file at {}".format(configs.pretrained_path)
model.load_state_dict(torch.load(configs.pretrained_path, map_location='cpu'))
print('Loaded weights from {}\n'.format(configs.pretrained_path))
configs.device = torch.device('cpu' if configs.no_cuda else 'cuda:{}'.format(configs.gpu_idx))
model = model.to(device=configs.device)
model.eval()
out_cap = None
demo_dataset = Demo_KittiDataset(configs)
with torch.no_grad():
for sample_idx in range(len(demo_dataset)):
metadatas, bev_map, img_rgb = demo_dataset.load_bevmap_front(sample_idx)
detections, bev_map, fps = do_detect(configs, model, bev_map, is_front=True)
# Draw prediction in the image
bev_map = (bev_map.permute(1, 2, 0).numpy() * 255).astype(np.uint8)
bev_map = cv2.resize(bev_map, (cnf.BEV_WIDTH, cnf.BEV_HEIGHT))
bev_map = draw_predictions(bev_map, detections, configs.num_classes)
# Rotate the bev_map
bev_map = cv2.rotate(bev_map, cv2.ROTATE_180)
img_path = metadatas['img_path'][0]
img_bgr = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR)
calib = Calibration(configs.calib_path)
kitti_dets = convert_det_to_real_values(detections)
if len(kitti_dets) > 0:
kitti_dets[:, 1:] = lidar_to_camera_box(kitti_dets[:, 1:], calib.V2C, calib.R0, calib.P2)
model = create_model(configs)
print('\n\n' + '-*=' * 30 + '\n\n')
assert os.path.isfile(configs.pretrained_path), "No file at {}".format(configs.pretrained_path)
model.load_state_dict(torch.load(configs.pretrained_path, map_location='cpu'))
print('Loaded weights from {}\n'.format(configs.pretrained_path))
configs.device = torch.device('cpu' if configs.no_cuda else 'cuda:{}'.format(configs.gpu_idx))
model = model.to(device=configs.device)
model.eval()
out_cap = None
demo_dataset = Demo_KittiDataset(configs)
with torch.no_grad():
for sample_idx in range(len(demo_dataset)):
metadatas, front_bevmap, back_bevmap, img_rgb = demo_dataset.load_bevmap_front_vs_back(sample_idx)
front_detections, front_bevmap, fps = do_detect(configs, model, front_bevmap, is_front=True)
back_detections, back_bevmap, _ = do_detect(configs, model, back_bevmap, is_front=False)
# Draw prediction in the image
front_bevmap = (front_bevmap.permute(1, 2, 0).numpy() * 255).astype(np.uint8)
front_bevmap = cv2.resize(front_bevmap, (cnf.BEV_WIDTH, cnf.BEV_HEIGHT))
front_bevmap = draw_predictions(front_bevmap, front_detections, configs.num_classes)
# Rotate the front_bevmap
front_bevmap = cv2.rotate(front_bevmap, cv2.ROTATE_90_COUNTERCLOCKWISE)
# Draw prediction in the image
back_bevmap = (back_bevmap.permute(1, 2, 0).numpy() * 255).astype(np.uint8)
back_bevmap = cv2.resize(back_bevmap, (cnf.BEV_WIDTH, cnf.BEV_HEIGHT))
back_bevmap = draw_predictions(back_bevmap, back_detections, configs.num_classes)
# Rotate the back_bevmap
back_bevmap = cv2.rotate(back_bevmap, cv2.ROTATE_90_CLOCKWISE)