def test():
corners_2d = []
dims = np.asarray([1.52563191462, 1.62856739989, 3.88311640418])
ground_plane = None
# depth = []
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)
# import ipdb
# ipdb.set_trace()
dataset = build_dataset()
for sample in dataset:
label_boxes_3d = sample[constants.KEY_LABEL_BOXES_3D]
num_instances = sample[constants.KEY_NUM_INSTANCES]
label_boxes_3d = label_boxes_3d[:num_instances]
corners_2d = geometry_utils.boxes_3d_to_corners_2d(label_boxes_3d, p2)
depth = label_boxes_3d[:, 2]
res = optimize_corners_2d(corners_2d[0],
dims,
p2,
ground_plane=ground_plane,
depth=depth[0])
print(res)
def func_given_depth(X):
"""
Args:
X: (x, y, ry)
"""
# import ipdb
# ipdb.set_trace()
# eight equations
x = X[0]
y = X[1]
# x = init_center_2d[0]
# y = init_center_2d[1]
if init_ry is not None:
ry = init_ry
depth_deltas = X[3]
dims_scale = X[4]
dims_rescale = dims
depth_refine = depth * dims_scale
boxes_3d = np.asarray([
x, y, depth_refine, dims_rescale[0], dims_rescale[1],
dims_rescale[2], ry
]).reshape(1, -1)
corners_2d_preds = geometry_utils.boxes_3d_to_corners_2d(boxes_3d, p2)
dist = corners_2d.reshape(-1) - corners_2d_preds.reshape(-1)
dist = np.linalg.norm(dist, p)
return dist
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 func_given_ground_plane(X):
"""
Args:
X: (x, y, ry)
"""
# import ipdb
# ipdb.set_trace()
# eight equations
x = X[0]
y = X[1]
ry = X[2]
boxes_3d = np.asarray([x, y, depth, dims[0], dims[1], dims[2],
ry]).reshape(1, -1)
corners_2d_preds = geometry_utils.boxes_3d_to_corners_2d(boxes_3d, p2)
dist = corners_2d.reshape(-1) - corners_2d_preds.reshape(-1)
dist = np.linalg.norm(dist, p)
return dist
def func_given_depth(X):
"""
Args:
X: (x, y, ry, dims)
"""
# import ipdb
# ipdb.set_trace()
# eight equations
x = X[0]
y = X[1]
h = X[2]
w = X[3]
l = X[4]
boxes_3d = np.asarray([x, y, depth, h, w, l, ry]).reshape(1, -1)
corners_2d_preds = geometry_utils.boxes_3d_to_corners_2d(boxes_3d, p2)
dist = corners_2d.reshape(-1) - corners_2d_preds.reshape(-1)
dist = np.linalg.norm(dist, p)
return dist
# -*- coding: utf-8 -*-
from utils import geometry_utils
import numpy as np
boxes_3d = np.asarray([[2.3900, 1.6000, 5.3700, 3.7800, 1.4200, 1.6000, 0.5 * np.pi]])
p2 = np.asarray([[721.5377, 0.0000, 609.5593, 44.8573],
[0.0000, 721.5377, 172.8540, 0.2164],
[0.0000, 0.0000, 1.0000, 0.0027]])
corners_2d = geometry_utils.boxes_3d_to_corners_2d(boxes_3d, p2)
boxes_2d = geometry_utils.corners_2d_to_boxes_2d(corners_2d)
print(boxes_2d)