def show_image_with_boxes(img,
objects_res,
object_gt,
calib,
save_path,
height_threshold=0):
img2 = np.copy(img)
for obj in objects_res:
box3d_pts_2d, _ = compute_box_3d(obj, calib.P)
color_tmp = tuple(
[int(tmp * 255) for tmp in colors[obj.id % max_color]])
img2 = draw_projected_box3d(img2, box3d_pts_2d, color=color_tmp)
text = 'ID: %d' % obj.id
if box3d_pts_2d is not None:
img2 = cv2.putText(
img2,
text,
(int(box3d_pts_2d[4, 0]), int(box3d_pts_2d[4, 1]) - 8),
cv2.FONT_HERSHEY_TRIPLEX,
0.5,
color=color_tmp)
img = Image.fromarray(img2)
img = img.resize((width, height))
img.save(save_path)
def show_image_with_boxes(self,
img,
objects_res,
calib,
save_path,
height_threshold=0):
img2 = np.copy(img)
for obj in objects_res:
#box3d_pts_2d = compute_box_3d(obj, calib.P, calib.V2C)
box3d_pts_2d = compute_box_3d(obj, calib.P, calib.V2C)
color_tmp = tuple(
[int(tmp * 255) for tmp in colors[obj.id % max_color]])
#print("box3d_pts_2d", box3d_pts_2d)
img2 = draw_projected_box3d(img2, box3d_pts_2d, color=color_tmp)
text = 'ID: %d' % obj.id # + ', s: %.2f' % obj.score
if box3d_pts_2d is not None:
img2 = cv2.putText(
img2,
text,
(int(box3d_pts_2d[4, 0]), int(box3d_pts_2d[4, 1]) - 8),
cv2.FONT_HERSHEY_TRIPLEX,
0.5,
color=color_tmp)
#img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2RGB)
#cv2.imwrite(save_path, img2)
self.image_pub.publish(self.bridge.cv2_to_imgmsg(img2, "rgb8"))
def show_lidar_with_boxes(
pc_velo,
objects,
calib,
img_fov=False,
img_width=None,
img_height=None,
objects_pred=None,
depth=None,
cam_img=None,
):
""" Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) """
if "mlab" not in sys.modules:
import mayavi.mlab as mlab
from viz_util import draw_lidar_simple, draw_lidar, draw_gt_boxes3d
print(("All point num: ", pc_velo.shape[0]))
fig = mlab.figure(figure=None,
bgcolor=(0, 0, 0),
fgcolor=None,
engine=None,
size=(1000, 500))
if img_fov:
pc_velo = get_lidar_in_image_fov(pc_velo[:, 0:3], calib, 0, 0,
img_width, img_height)
print(("FOV point num: ", pc_velo.shape[0]))
print("pc_velo", pc_velo.shape)
draw_lidar(pc_velo, fig=fig)
# pc_velo=pc_velo[:,0:3]
color = (0, 1, 0)
for obj in objects:
if obj.type == "DontCare":
continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
draw_gt_boxes3d([box3d_pts_3d_velo],
fig=fig,
color=color,
label=obj.type)
# Draw depth
if depth is not None:
# import pdb; pdb.set_trace()
depth_pt3d = depth_region_pt3d(depth, obj)
depth_UVDepth = np.zeros_like(depth_pt3d)
depth_UVDepth[:, 0] = depth_pt3d[:, 1]
depth_UVDepth[:, 1] = depth_pt3d[:, 0]
depth_UVDepth[:, 2] = depth_pt3d[:, 2]
print("depth_pt3d:", depth_UVDepth)
dep_pc_velo = calib.project_image_to_velo(depth_UVDepth)
print("dep_pc_velo:", dep_pc_velo)
draw_lidar(dep_pc_velo, fig=fig, pts_color=(1, 1, 1))
#
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = compute_orientation_3d(obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1, y1, z1 = ori3d_pts_3d_velo[0, :]
x2, y2, z2 = ori3d_pts_3d_velo[1, :]
mlab.plot3d(
[x1, x2],
[y1, y2],
[z1, z2],
color=color,
tube_radius=None,
line_width=1,
figure=fig,
)
if objects_pred is not None:
color = (1, 0, 0)
for obj in objects_pred:
if obj.type == "DontCare":
continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig, color=color)
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(
obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1, y1, z1 = ori3d_pts_3d_velo[0, :]
x2, y2, z2 = ori3d_pts_3d_velo[1, :]
mlab.plot3d(
[x1, x2],
[y1, y2],
[z1, z2],
color=color,
tube_radius=None,
line_width=1,
figure=fig,
)
mlab.show(1)
filecontent = np.array([f.split() for f in open(file_path, 'r')])
alllabels = np.unique(filecontent[:, 2])
labels = ['Car', 'Pedestrian', 'Cyclist']
finalset = [x for x in alllabels if x not in labels]
for val in finalset:
filecontent = filecontent[filecontent[:, 2] != val, :]
data = (Object3d(getstringfromarray(line[2:]))
for line in filecontent[filecontent[:, 0] == '0', :])
calib = Calibration('/home/eshan/Pictures/calib.txt')
velo = load_velo_scan('/home/eshan/Pictures/000009.bin',
np.float32,
n_vec=4)[:, 0:4]
img2 = np.copy(img)
show_lidar_with_boxes(velo, data, calib)
for obj in data:
print("here")
box3d_pts_2d, _ = compute_box_3d(obj, calib.P)
img2 = draw_projected_box3d(img2, box3d_pts_2d, color=(0, 0, 0))
text = 'gt ID: %d, Type: %s' % (0, obj.type)
if box3d_pts_2d is not None:
print("also")
img2 = cv2.putText(
img2,
text, (int(box3d_pts_2d[4, 0]), int(box3d_pts_2d[4, 1]) - 8),
cv2.FONT_HERSHEY_TRIPLEX,
0.5,
color=(0, 0, 0))
# img = Image.fromarray(img2)
# img = img.resize((width, height))
cv2.imshow("image", img2)
cv2.waitKey()
def show_image_with_boxes(img,
velo,
objects_res,
objects_res_det,
objects_res_raw,
labeldata,
object_gt,
calib,
save_path,
height_threshold=0,
show_lidar=True,
save_image=False):
img2 = np.copy(img)
for obj in objects_res:
box3d_pts_2d, _ = compute_box_3d(obj, calib.P)
color_tmp = tuple(
[int(tmp * 255) for tmp in colors[obj.id % max_color]])
img2 = draw_projected_box3d(img2, box3d_pts_2d, color=(0, 0, 255))
text = 'Tracked ID: %d, Type: %s' % (obj.id, obj.type)
if box3d_pts_2d is not None:
img2 = cv2.putText(
img2,
text,
(int(box3d_pts_2d[4, 0]), int(box3d_pts_2d[4, 1]) - 8),
cv2.FONT_HERSHEY_TRIPLEX,
0.5,
color=(0, 0, 255))
for obj in objects_res_det:
box3d_pts_2d, _ = compute_box_3d(obj, calib.P)
color_tmp = tuple(
[int(tmp * 255) for tmp in colors[obj.id % max_color]])
img2 = draw_projected_box3d(img2, box3d_pts_2d, color=(0, 255, 0))
text = 'Detection ID: %d, Type: %s' % (obj.id, obj.type)
if box3d_pts_2d is not None:
img2 = cv2.putText(
img2,
text,
(int(box3d_pts_2d[3, 0]), int(box3d_pts_2d[3, 1]) - 8),
cv2.FONT_HERSHEY_TRIPLEX,
0.5,
color=(0, 255, 0))
import itertools
labeldata, labeldata2 = itertools.tee(labeldata)
for obj in labeldata:
# print("here")
box3d_pts_2d, _ = compute_box_3d(obj, calib.P)
img2 = draw_projected_box3d(img2, box3d_pts_2d, color=(255, 0, 0))
text = 'GT, Type: %s' % (obj.type)
if box3d_pts_2d is not None:
# print("also")
print(text)
img2 = cv2.putText(
img2,
text,
(int(box3d_pts_2d[4, 0]), int(box3d_pts_2d[4, 1]) - 8),
cv2.FONT_HERSHEY_TRIPLEX,
0.5,
color=(255, 0, 0))
# for obj in objects_res_raw:
# box3d_pts_2d, _ = compute_box_3d(obj, calib.P)
# color_tmp = tuple([int(tmp * 255)
# for tmp in colors[obj.id % max_color]])
# img2 = draw_projected_box3d(img2, box3d_pts_2d, color=(255,0,0))
# text = 'Estimate ID: %d' % obj.id
# if box3d_pts_2d is not None:
# img2 = cv2.putText(img2, text, (int(box3d_pts_2d[2, 0]), int(
# box3d_pts_2d[2, 1]) - 8), cv2.FONT_HERSHEY_TRIPLEX, 0.5, color=(255,0,0))
if show_lidar:
show_lidar_with_boxes(velo,
objects=labeldata2,
calib=calib,
objects_pred=objects_res)
img = Image.fromarray(img2)
img = img.resize((width, height))
cv2.imshow("Image", img2)
cv2.waitKey()
if save_image:
print("Saving Image at", save_path)
img.save(save_path)
return img2
def __getitem__(self, index):
# start time
get_item_start_time = time.time()
# get point cloud
lidar_filename = os.path.join(self.lidar_dir, '%06d.bin' % index)
lidar_data = kitti_utils.load_velo_scan(lidar_filename)
# time for loading point cloud
read_point_cloud_end_time = time.time()
read_point_cloud_time = read_point_cloud_end_time - get_item_start_time
# voxelize point cloud
voxel_point_cloud = torch.tensor(
kitti_utils.voxelize(point_cloud=lidar_data),
requires_grad=True,
device=self.device).float()
# time for voxelization
voxelization_end_time = time.time()
voxelization_time = voxelization_end_time - read_point_cloud_end_time
# channels along first dimensions according to PyTorch convention
voxel_point_cloud = voxel_point_cloud.permute([2, 0, 1])
# get image
if self.get_image:
image_filename = os.path.join(self.image_dir, '%06d.png' % index)
image = kitti_utils.get_image(image_filename)
# get current time
read_labels_start_time = time.time()
# get calibration
calib_filename = os.path.join(self.calib_dir, '%06d.txt' % index)
calib = kitti_utils.Calibration(calib_filename)
# get labels
label_filename = os.path.join(self.label_dir, '%06d.txt' % index)
labels = kitti_utils.read_label(label_filename)
read_labels_end_time = time.time()
read_labels_time = read_labels_end_time - read_labels_start_time
# compute network label
if self.split == 'training':
# get current time
compute_label_start_time = time.time()
# create empty pixel labels
regression_label = np.zeros(
(OUTPUT_DIM_0, OUTPUT_DIM_1, OUTPUT_DIM_REG))
classification_label = np.zeros(
(OUTPUT_DIM_0, OUTPUT_DIM_1, OUTPUT_DIM_CLA))
# iterate over all 3D label objects in list
for label in labels:
if label.type == 'Car':
# compute corners of 3D bounding box in camera coordinates
_, bbox_corners_camera_coord = kitti_utils.compute_box_3d(
label, calib.P, scale=1.0)
# get pixel label for classification and BEV bounding box
regression_label, classification_label = compute_pixel_labels\
(regression_label, classification_label, label, bbox_corners_camera_coord)
# stack classification and regression label
regression_label = torch.tensor(regression_label,
device=self.device).float()
classification_label = torch.tensor(classification_label,
device=self.device).float()
training_label = torch.cat(
(regression_label, classification_label), dim=2)
# get time for computing pixel label
compute_label_end_time = time.time()
compute_label_time = compute_label_end_time - compute_label_start_time
# total time for data loading
get_item_end_time = time.time()
get_item_time = get_item_end_time - get_item_start_time
if self.show_times:
print('---------------------------')
print('Get Item Time: {:.4f} s'.format(get_item_time))
print('---------------------------')
print('Read Point Cloud Time: {:.4f} s'.format(
read_point_cloud_time))
print('Voxelization Time: {:.4f} s'.format(voxelization_time))
print('Read Labels Time: {:.4f} s'.format(read_labels_time))
print(
'Compute Labels Time: {:.4f} s'.format(compute_label_time))
return voxel_point_cloud, training_label
else:
if self.get_image:
return image, voxel_point_cloud, labels, calib
else:
return voxel_point_cloud, labels, calib
