def preprocesiing(self, pointcloud):
pointcloud = get_filtered_lidar(pointcloud, cnf.boundary)
bev = makeBEVMap(pointcloud, cnf.boundary)
bev = torch.from_numpy(bev)
bev = torch.unsqueeze(bev, 0)
bev = bev.to(self.configs.device, non_blocking=True).float()
return bev
def load_img_with_targets(self, index):
"""Load images and targets for the training and validation phase"""
sample_id = int(self.sample_id_list[index])
img_path = os.path.join(self.image_dir, '{:06d}.png'.format(sample_id))
lidarData = self.get_lidar(sample_id)
calib = self.get_calib(sample_id)
labels, has_labels = self.get_label(sample_id)
if has_labels:
labels[:, 1:] = transformation.camera_to_lidar_box(
labels[:, 1:], calib.V2C, calib.R0, calib.P2)
if self.lidar_aug:
lidarData, labels[:, 1:] = self.lidar_aug(lidarData, labels[:, 1:])
lidarData, labels = get_filtered_lidar(lidarData, cnf.boundary, labels)
bev_map = makeBEVMap(lidarData, cnf.boundary)
bev_map = torch.from_numpy(bev_map)
hflipped = False
if np.random.random() < self.hflip_prob:
hflipped = True
# C, H, W
bev_map = torch.flip(bev_map, [-1])
targets = self.build_targets(labels, hflipped)
metadatas = {'img_path': img_path, 'hflipped': hflipped}
return metadatas, bev_map, targets
def complex_yolo(pointcloud):
pointcloud = get_filtered_lidar(pointcloud, cnf.boundary)
bev_maps = makeBEVMap(pointcloud, cnf.boundary)
bev_maps = torch.from_numpy(bev_maps)
bev_maps = torch.unsqueeze(bev_maps, 0)
input_bev_maps = bev_maps.to(configs.device, non_blocking=True).float()
t1 = time_synchronized()
outputs = model(input_bev_maps)
outputs['hm_cen'] = _sigmoid(outputs['hm_cen'])
outputs['cen_offset'] = _sigmoid(outputs['cen_offset'])
# detections size (batch_size, K, 10)
detections = decode(outputs['hm_cen'], outputs['cen_offset'], outputs['direction'], outputs['z_coor'],outputs['dim'], K=configs.K)
detections = detections.cpu().detach().numpy().astype(np.float32)
detections = post_processing(detections, configs.num_classes, configs.down_ratio, configs.peak_thresh)
t2 = time_synchronized()
detections = detections[0] # only first batch
# Draw prediction in the image
bev_map = (bev_maps.squeeze().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.copy(), configs.num_classes)
bev_map = cv2.rotate(bev_map, cv2.ROTATE_180)
cv2.imshow("BEV", bev_map)
print('\tDone testing in time: {:.1f}ms, speed {:.2f}FPS'.format((t2 - t1) * 1000,1 / (t2 - t1)))
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)
def load_bevmap_front_vs_back(self, index):
"""Load only image for the testing phase"""
sample_id = int(self.sample_id_list[index])
img_path, img_rgb = self.get_image(sample_id)
lidarData = self.get_lidar(sample_id)
front_lidar = get_filtered_lidar(lidarData, cnf.boundary)
front_bevmap = makeBEVMap(front_lidar, cnf.boundary)
front_bevmap = torch.from_numpy(front_bevmap)
back_lidar = get_filtered_lidar(lidarData, cnf.boundary_back)
back_bevmap = makeBEVMap(back_lidar, cnf.boundary_back)
back_bevmap = torch.from_numpy(back_bevmap)
metadatas = {
'img_path': img_path,
}
return metadatas, front_bevmap, back_bevmap, img_rgb
def load_img_only(self, index):
"""Load only image for the testing phase"""
sample_id = int(self.sample_id_list[index])
img_path, img_rgb = self.get_image(sample_id)
lidarData = self.get_lidar(sample_id)
lidarData = get_filtered_lidar(lidarData, cnf.boundary)
bev_map = makeBEVMap(lidarData, cnf.boundary)
bev_map = torch.from_numpy(bev_map)
metadatas = {
'img_path': img_path,
}
return metadatas, bev_map, img_rgb
def draw_img_with_label(self, index):
sample_id = int(self.sample_id_list[index])
img_path, img_rgb = self.get_image(sample_id)
lidarData = self.get_lidar(sample_id)
calib = self.get_calib(sample_id)
labels, has_labels = self.get_label(sample_id)
if has_labels:
labels[:, 1:] = transformation.camera_to_lidar_box(labels[:, 1:], calib.V2C, calib.R0, calib.P2)
if self.lidar_aug:
lidarData, labels[:, 1:] = self.lidar_aug(lidarData, labels[:, 1:])
lidarData, labels = get_filtered_lidar(lidarData, cnf.boundary, labels)
bev_map = makeBEVMap(lidarData, cnf.boundary)
return bev_map, labels, img_rgb, img_path
def callback(self, data):
rospy.loginfo("detection")
with torch.no_grad():
gen = point_cloud2.read_points(data)
#print(type(gen))
cloudata = []
for idx, p in enumerate(gen):
data = np.array([p[0], p[1], p[2], p[3]])
data = data.reshape((1, 4))
cloudata.append(data)
lidarData = np.concatenate([x for x in cloudata], axis=0)
lidarData = get_filtered_lidar(lidarData, cnf.boundary)
res = self.voxel_generator.generate(lidarData, 20000)
coorinput = np.pad(res["coordinates"], ((0, 0), (1, 0)),
mode='constant',
constant_values=0)
voxelinput = res["voxels"]
numinput = res["num_points_per_voxel"]
dtype = torch.float32
voxelinputr = torch.tensor(voxelinput,
dtype=torch.float32,
device=configs.device).to(dtype)
coorinputr = torch.tensor(coorinput,
dtype=torch.int32,
device=configs.device)
numinputr = torch.tensor(numinput,
dtype=torch.int32,
device=configs.device)
outputs = self.model(voxelinputr, coorinputr, numinputr)
outputs = outputs._asdict()
outputs['hm_cen'] = _sigmoid(outputs['hm_cen'])
outputs['cen_offset'] = _sigmoid(outputs['cen_offset'])
# detections size (batch_size, K, 10)
detections = decode(outputs['hm_cen'],
outputs['cen_offset'],
outputs['direction'],
outputs['z_coor'],
outputs['dim'],
K=configs.K)
detections = detections.cpu().numpy().astype(np.float32)
detections = post_processing(detections, configs.num_classes,
configs.down_ratio,
configs.peak_thresh)
detections = detections[0]
bev_map = np.ones((432, 432, 3), dtype=np.uint8)
bev_map = bev_map * 0.5
bev_map = makeBEVMap(lidarData, cnf.boundary)
bev_map = bev_map.transpose((1, 2, 0)) * 255
#bev_map = (bev_maps.squeeze().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.copy(),
configs.num_classes)
# Rotate the bev_map
bev_map = cv2.rotate(bev_map, cv2.ROTATE_180)
out_img = bev_map
cv2.imshow('test-img', out_img)
cv2.waitKey(1)
# Draw prediction in the image
bev_map = np.ones((432, 432, 3), dtype=np.uint8)
bev_map = bev_map * 0.5
coors = coorinput
img_path = metadatas['img_path'][0]
from data_process.kitti_data_utils import get_filtered_lidar
pointcloud = get_lidar(
img_path.replace(".png",
".bin").replace("image_2", "velodyne"))
lidardata = get_filtered_lidar(pointcloud, cnf.boundary)
from data_process.kitti_bev_utils import makeBEVMap
bev_map = makeBEVMap(lidardata, cnf.boundary)
bev_map = bev_map.transpose((1, 2, 0)) * 255
bev_map = cv2.resize(bev_map, (cnf.BEV_WIDTH, cnf.BEV_HEIGHT))
bev_map = draw_predictions(bev_map, detections.copy(),
configs.num_classes)
# Rotate the bev_map
bev_map = cv2.rotate(bev_map, cv2.ROTATE_180)
img_rgb = metadatas['img_rgb'][0]
img_rgb = cv2.resize(img_rgb, (img_rgb.shape[1], img_rgb.shape[0]))
img_bgr = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR)
calib = Calibration(
img_path.replace(".png", ".txt").replace("image_2", "calib"))
kitti_dets = convert_det_to_real_values(detections,