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)