def convert_to_corners_3d(dims, depth, xyry):
# import ipdb
# ipdb.set_trace()
x, y, ry = xyry[:3]
dims_scale = xyry[-1]
dims = dims * dims_scale
label_boxes_3d = np.asarray([x, y, depth, dims[0], dims[1], dims[2],
ry]).reshape(-1, 7)
corners_3d = geometry_utils.boxes_3d_to_corners_3d(label_boxes_3d)
return corners_3d
def render_image_3d(self, image, boxes_3d, label_classes, p2):
connected_points = [[2, 4, 5], [1, 3, 6], [2, 4, 7], [1, 3, 8],
[1, 6, 8], [2, 5, 7], [3, 6, 8], [4, 5, 7]]
connected_points = np.array(connected_points) - 1
connected_points = connected_points.tolist()
# connected_points_2d = [[1, 3], [0, 2], [1, 3], [0, 2]]
# parse calib first
corners_3d = geometry_utils.boxes_3d_to_corners_3d(boxes_3d[:, :-1])
corners_2d = geometry_utils.points_3d_to_points_2d(
corners_3d.reshape((-1, 3)), p2).reshape(-1, 8, 2)
# bev image
voxel_size = 0.05
width = 80
height = 75
bev_width = int(height / voxel_size)
bev_height = int(width / voxel_size)
connected_points_2d = [[1, 3], [0, 2], [1, 3], [0, 2]]
bev_image = np.zeros((bev_height, bev_width, 3), np.uint8)
bev_image[...] = 255
corners_2d = corners_2d.astype(np.int32).tolist()
for i in range(boxes_3d.shape[0]):
color = self.colors[label_classes[i]]
corners_image = corners_2d[i]
corners_bird = corners_3d[i][:4, [0, 2]]
corners_bird = corners_bird[:, ::-1]
corners_bird[:, 1] = corners_bird[:, 1] + 1 / 2 * width
corners_bird = (corners_bird / voxel_size).astype(np.int)
# render box in bev view
for i in range(4):
for j in range(4):
if j in connected_points_2d[i]:
start_point = (corners_bird[i][0], corners_bird[i][1])
end_point = (corners_bird[j][0], corners_bird[j][1])
cv2.line(bev_image, start_point, end_point, color, 2)
# render box in image
for i in range(8):
for j in range(8):
if j in connected_points[i]:
start_point = (corners_image[i][0],
corners_image[i][1])
end_point = (corners_image[j][0], corners_image[j][1])
cv2.line(image, start_point, end_point, color, 2)
# rotate bev_image
bev_image = cv2.rotate(bev_image, cv2.ROTATE_90_COUNTERCLOCKWISE)
return image, bev_image
def generate_mid_line(boxes_3d, p2):
"""
Args:
boxes_3d: shape(7), (xyz,hwl, ry)
"""
boxes_3d = np.asarray(boxes_3d).reshape(1, -1)
corners_3d = geometry_utils.boxes_3d_to_corners_3d(boxes_3d)
# find the nearest line
dist = np.linalg.norm(corners_3d, axis=-1)
argmin = dist.argmin(axis=-1)
corners_2d = geometry_utils.boxes_3d_to_corners_2d(boxes_3d, p2)
row = np.arange(corners_2d.shape[0])
mid_line_2d_tmp = corners_2d[row, argmin]
return mid_line_2d_tmp[0]
def mainv2():
"""
self optimization
"""
label_dir = 'results/data'
corners_label_dir = 'results/corners'
p2 = np.asarray([
7.070493000000e+02, 0.000000000000e+00, 6.040814000000e+02,
4.575831000000e+01, 0.000000000000e+00, 7.070493000000e+02,
1.805066000000e+02, -3.454157000000e-01, 0.000000000000e+00,
0.000000000000e+00, 1.000000000000e+00, 4.981016000000e-03
]).reshape(3, 4)
visualizer = generate_visualizer()
image_dir = '/data/object/training/image_2'
for sample_ind, sample_name in enumerate(
sorted(os.listdir(corners_label_dir))):
# sample_name = '000006.txt'
corners_label_path = os.path.join(corners_label_dir, sample_name)
labels = read_txt(corners_label_path)
num_instances = labels.shape[0]
boxes_2d = labels[:, :4]
corners_2d = labels[:, 4:4 + 16]
dims = labels[:, 4 + 16:4 + 16 + 3]
depth = labels[:, 4 + 16 + 3:4 + 16 + 3 + 1]
conf = labels[:, -1:]
ground_plane = None
center_2d = (boxes_2d[:, :2] + boxes_2d[:, 2:]) / 2
points_3d = geometry_utils.points_2d_to_points_3d(center_2d, depth, p2)
center_2d = points_3d[:, :2]
# import ipdb
# ipdb.set_trace()
ry = estimate_ry(corners_2d, p2)
# location = estimate_location(corners_2d, depth, p2)
# optimize xy
optimized_xys = []
for ind in range(num_instances):
optimized_xys.append(
optimize_xy(center_2d[ind], [1, 1, 1],
p2,
ry[ind, 0],
depth[ind, 0],
corners_2d[ind],
p=2).x[:2])
optimized_xys = np.asarray(optimized_xys).reshape(-1, 2)
# optimize rys
# import ipdb
# ipdb.set_trace()
optimized_rys = []
for ind in range(num_instances):
optimized_rys.append(
optimize_ry(optimized_xys[ind],
dims[ind],
p2,
ry[ind, 0],
depth[ind, 0],
corners_2d[ind],
p=1).x[0])
optimized_rys = np.asarray(optimized_rys).reshape(-1, 1)
boxes_3d = np.concatenate([optimized_xys[:, :2], depth, dims, ry],
axis=-1)
corners_3d = geometry_utils.boxes_3d_to_corners_3d(boxes_3d)
# corners_3d = np.concatenate(corners_3d, axis=0)
image_path = os.path.join(image_dir, '{}.png'.format(sample_name[:-4]))
visualizer.render_image_corners_2d(image_path,
corners_2d=None,
boxes_2d=None,
corners_3d=corners_3d,
p2=p2)
dets = np.concatenate([boxes_2d, boxes_3d, conf], axis=-1)
# save to txt
label_path = os.path.join(label_dir, '{}.txt'.format(sample_name[:-4]))
save_dets(dets, label_path)
sys.stdout.write('\rind: {} sample_name: {}'.format(
sample_ind, sample_name))
sys.stdout.flush()
def box_3d_filter(box_3d, pointcloud):
# planes = geometry_utils.boxes_3d_to_plane(box_3d[None])[0]
corners_3d = geometry_utils.boxes_3d_to_corners_3d(box_3d[None])[0]
point_mask = obj_utils.is_point_inside(pointcloud, corners_3d.T)
return pointcloud.T[point_mask].T