def compute_box_3d(dim_changed, location_changed, rotation_y):
c, s = np.cos(rotation_y), np.sin(rotation_y)
R = np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], dtype=np.float32)
rot_inds = order_rotation_y_matrix(R)
rot_mat = R[rot_inds, :]
x = dim_changed[0]
y = dim_changed[1]
z = dim_changed[2]
x_corners = [-x / 2, x / 2, x / 2, -x / 2, -x / 2, x / 2, x / 2, -x / 2]
y_corners = [-y / 2, -y / 2, -y / 2, -y / 2, y / 2, y / 2, y / 2, y / 2]
z_corners = [z / 2, z / 2, -z / 2, -z / 2, z / 2, z / 2, -z / 2, -z / 2]
# x_corners = [x/2, x/2, -x/2, -x/2, x/2, x/2, -x/2, -x/2]
# # y_corners = [y/2, y/2, y/2, y/2, -y/2, -y/2, -y/2, -y/2]
# y_corners = [0, 0, 0, 0, -y, -y, -y, -y]
# z_corners = [z/2, -z/2, -z/2, z/2, z/2, -z/2, -z/2, z/2]
#
corners = np.array([x_corners, y_corners, z_corners], dtype=np.float32)
corners_3d = np.dot(
rot_mat, corners) # R_rot *x_ref_coord(camera_coordinate에서의 좌표값들)
temp = np.array(location_changed, dtype=np.float32).reshape(3, 1)
corners_3d = corners_3d + np.array(
location_changed, dtype=np.float32).reshape(
3, 1) # camera좌표 (0,0)에서 시작했었으니까, 물체 center point로 평행이동시킴
return corners_3d.transpose(1, 0)
def compute_box_3d_Rtilt(dim_changed, location_changed, rotation_y, Rtilt):
c, s = np.cos(rotation_y), np.sin(rotation_y)
R = np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], dtype=np.float32)
rot_inds = order_rotation_y_matrix(R)
rot_mat = R[rot_inds, :]
x = dim_changed[0]
y = dim_changed[1]
z = dim_changed[2]
x_corners = [-x / 2, x / 2, x / 2, -x / 2, -x / 2, x / 2, x / 2, -x / 2]
y_corners = [-y / 2, -y / 2, -y / 2, -y / 2, y / 2, y / 2, y / 2, y / 2]
z_corners = [z / 2, z / 2, -z / 2, -z / 2, z / 2, z / 2, -z / 2, -z / 2]
# x_corners = [x/2, x/2, -x/2, -x/2, x/2, x/2, -x/2, -x/2]
# # y_corners = [y/2, y/2, y/2, y/2, -y/2, -y/2, -y/2, -y/2]
# y_corners = [0, 0, 0, 0, -y, -y, -y, -y]
# z_corners = [z/2, -z/2, -z/2, z/2, z/2, -z/2, -z/2, z/2]
#
corners = np.array([x_corners, y_corners, z_corners], dtype=np.float32)
corners_3d = np.dot(
rot_mat, corners) # R_rot *x_ref_coord(camera_coordinate에서의 좌표값들)
temp = np.array(location_changed, dtype=np.float32).reshape(3, 1)
corners_3d = corners_3d + np.array(
location_changed, dtype=np.float32).reshape(
3, 1) # camera좌표 (0,0)에서 시작했었으니까, 물체 center point로 평행이동시킴
Rtilt = np.transpose(Rtilt)
Rtilt_revised = np.zeros((3, 3))
Rtilt_revised[0, 0] = Rtilt[0, 0]
Rtilt_revised[0, 1] = -Rtilt[0, 2]
Rtilt_revised[0, 2] = Rtilt[0, 1]
Rtilt_revised[1, 0] = -Rtilt[2, 0]
Rtilt_revised[1, 1] = Rtilt[2, 2]
Rtilt_revised[1, 2] = -Rtilt[2, 1]
Rtilt_revised[2, 0] = Rtilt[1, 0]
Rtilt_revised[2, 1] = -Rtilt[1, 2]
Rtilt_revised[2, 2] = Rtilt[1, 1]
corners_3d = np.dot(Rtilt_revised, corners_3d) #(3,8)
inversed_Rtilt_revised = inv(Rtilt_revised)
corners_3d = np.dot(inversed_Rtilt_revised, corners_3d)
return corners_3d.transpose(1, 0)
def compute_box_rtilt_xzy(dim, location, rotation_y, Rtilt):
c, s = np.cos(rotation_y), np.sin(rotation_y)
R = np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], dtype=np.float32)
rot_inds = order_rotation_y_matrix(R)
rot_mat = R[rot_inds, :]
x = dim[0]
y = dim[1]
z = dim[2]
x_corners = [-x / 2, x / 2, x / 2, -x / 2, -x / 2, x / 2, x / 2, -x / 2]
y_corners = [y / 2, y / 2, y / 2, y / 2, -y / 2, -y / 2, -y / 2, -y / 2]
z_corners = [z / 2, z / 2, -z / 2, -z / 2, z / 2, z / 2, -z / 2, -z / 2]
corners = np.array([x_corners, y_corners, z_corners], dtype=np.float32)
print("corners")
print(corners)
corners_3d = np.dot(rot_mat, corners) # R_rot *x_ref_coord(camera_coordinate에서의 좌표값들)
temp = np.array(location, dtype=np.float32).reshape(3, 1)
corners_3d = corners_3d + np.array(location, dtype=np.float32).reshape(3,1) #(3,8)
Rtilt = np.transpose(Rtilt)
Rtilt_revised = np.zeros((3, 3))
Rtilt_revised[0, 0] = Rtilt[0, 0]
Rtilt_revised[0, 1] = -Rtilt[0, 2]
Rtilt_revised[0, 2] = Rtilt[0, 1]
Rtilt_revised[1, 0] = -Rtilt[2, 0]
Rtilt_revised[1, 1] = Rtilt[2, 2]
Rtilt_revised[1, 2] = -Rtilt[2, 1]
Rtilt_revised[2, 0] = Rtilt[1, 0]
Rtilt_revised[2, 1] = -Rtilt[1, 2]
Rtilt_revised[2, 2] = Rtilt[1, 1]
inversed_Rtilt_revised = inv(Rtilt_revised)
# location_inversed_Rtilt_revised = np.dot(inversed_Rtilt_revised, location)
# corners_3d = corners_3d + np.array(location_inversed_Rtilt_revised, dtype=np.float32).reshape(3, 1)
corners_3d = np.dot(Rtilt_revised, corners_3d)
# corners_3d = corners_3d.transpose(1, 0)
# corners_3d = np.dot(Rtilt, corners_3d)
return corners_3d.transpose(1, 0)
