def np_box_3d_to_box_8co(box_3d): """Computes the 3D bounding box corner positions from Box3D format. The order of corners are preserved during this conversion. Args: box_3d: 1 x 7 ndarray of box_3d in the format [x, y, z, l, w, h, ry] Returns: corners_3d: An ndarray or a tensor of shape (3 x 8) representing the box as corners in the following format -> [[x1,...,x8], [y1...,y8], [z1,...,z8]]. """ format_checker.check_box_3d_format(box_3d) ry = box_3d[6] # Compute transform matrix # This includes rotation and translation rot = np.array([[np.cos(ry), 0, np.sin(ry), box_3d[0]], [0, 1, 0, box_3d[1]], [-np.sin(ry), 0, np.cos(ry), box_3d[2]]]) length = box_3d[3] width = box_3d[4] height = box_3d[5] # 3D BB corners x_corners = np.array([ length / 2, length / 2, -length / 2, -length / 2, length / 2, length / 2, -length / 2, -length / 2 ]) y_corners = np.array( [0.0, 0.0, 0.0, 0.0, -height, -height, -height, -height]) z_corners = np.array([ width / 2, -width / 2, -width / 2, width / 2, width / 2, -width / 2, -width / 2, width / 2 ]) # Create a ones column ones_col = np.ones(x_corners.shape) # Append the column of ones to be able to multiply box_8c = np.dot(rot, np.array([x_corners, y_corners, z_corners, ones_col])) # Ignore the fourth column box_8c = box_8c[0:3] return box_8c
def box_3d_to_anchor(boxes_3d, ortho_rotate=False): """ Converts a box_3d [x, y, z, l, w, h, ry] into anchor form [x, y, z, dim_x, dim_y, dim_z] Anchors in box_3d format should have an ry of 0 or 90 degrees. l and w will be matched to dim_x or dim_z depending on the rotation, while h will always correspond to dim_y Args: boxes_3d: N x 7 ndarray of box_3d ortho_rotate: optional, if True the box is rotated to the nearest 90 degree angle, or else the box is projected onto the x and z axes Returns: N x 6 ndarray of anchors in 'anchor' form """ boxes_3d = np.asarray(boxes_3d).reshape(-1, 7) fc.check_box_3d_format(boxes_3d) num_anchors = len(boxes_3d) anchors = np.zeros((num_anchors, 6)) # Set x, y, z anchors[:, [0, 1, 2]] = boxes_3d[:, [0, 1, 2]] # Dimensions along x, y, z box_l = boxes_3d[:, [3]] box_w = boxes_3d[:, [4]] box_h = boxes_3d[:, [5]] box_ry = boxes_3d[:, [6]] # Rotate to nearest multiple of 90 degrees if ortho_rotate: half_pi = np.pi / 2 box_ry = np.round(box_ry / half_pi) * half_pi cos_ry = np.abs(np.cos(box_ry)) sin_ry = np.abs(np.sin(box_ry)) # dim_x, dim_y, dim_z anchors[:, [3]] = box_l * cos_ry + box_w * sin_ry anchors[:, [4]] = box_h anchors[:, [5]] = box_w * cos_ry + box_l * sin_ry return anchors
def box_3d_to_3d_iou_format(boxes_3d): """ Returns a numpy array of 3d box format for iou calculation Args: boxes_3d: list of 3d boxes Returns: new_anchor_list: numpy array of 3d box format for iou """ boxes_3d = np.asarray(boxes_3d) fc.check_box_3d_format(boxes_3d) iou_3d_boxes = np.zeros([len(boxes_3d), 7]) iou_3d_boxes[:, 4:7] = boxes_3d[:, 0:3] iou_3d_boxes[:, 1] = boxes_3d[:, 3] iou_3d_boxes[:, 2] = boxes_3d[:, 4] iou_3d_boxes[:, 3] = boxes_3d[:, 5] iou_3d_boxes[:, 0] = boxes_3d[:, 6] return iou_3d_boxes
def box_3d_to_object_label(box_3d, obj_type='Car'): """Turns a box_3d into an ObjectLabel Args: box_3d: 3D box in the format [x, y, z, l, w, h, ry] obj_type: Optional, the object type Returns: ObjectLabel with the location, size, and rotation filled out """ fc.check_box_3d_format(box_3d) obj_label = obj_utils.ObjectLabel() obj_label.type = obj_type obj_label.t = box_3d.take((0, 1, 2)) obj_label.l = box_3d[3] obj_label.w = box_3d[4] obj_label.h = box_3d[5] obj_label.ry = box_3d[6] return obj_label
def test_check_box_3d_format(self): # Case 1, invalid type test_var = [0, 0, 0, 0, 0, 0, 0] np.testing.assert_raises(TypeError, fc.check_box_3d_format, test_var) # Case 2, invalid shape test_var = np.ones([1, 5]) np.testing.assert_raises(TypeError, fc.check_box_3d_format, test_var) test_var = np.ones([5, 6]) np.testing.assert_raises(TypeError, fc.check_box_3d_format, test_var) test_var = np.ones([1, 7]) fc.check_box_3d_format(test_var) test_var = np.ones([10, 7]) fc.check_box_3d_format(test_var) test_var = tf.ones([5, 7]) fc.check_box_3d_format(test_var) test_var = tf.ones([5, 3]) np.testing.assert_raises(TypeError, fc.check_box_3d_format, test_var)
def project_to_image_space(box_3d, calib_p2, truncate=False, image_size=None, discard_before_truncation=True): """ Projects a box_3d into image space Args: box_3d: single box_3d to project calib_p2: stereo calibration p2 matrix truncate: if True, 2D projections are truncated to be inside the image image_size: [w, h] must be provided if truncate is True, used for truncation discard_before_truncation: If True, discard boxes that are larger than 80% of the image in width OR height BEFORE truncation. If False, discard boxes that are larger than 80% of the width AND height AFTER truncation. Returns: Projected box in image space [x1, y1, x2, y2] Returns None if box is not inside the image """ format_checker.check_box_3d_format(box_3d) obj_label = box_3d_encoder.box_3d_to_object_label(box_3d) corners_3d = obj_utils.compute_box_corners_3d(obj_label) projected = calib_utils.project_to_image(corners_3d, calib_p2) x1 = np.amin(projected[0]) y1 = np.amin(projected[1]) x2 = np.amax(projected[0]) y2 = np.amax(projected[1]) img_box = np.array([x1, y1, x2, y2]) if truncate: if not image_size: raise ValueError('Image size must be provided') image_w = image_size[0] image_h = image_size[1] # Discard invalid boxes (outside image space) if img_box[0] > image_w or \ img_box[1] > image_h or \ img_box[2] < 0 or \ img_box[3] < 0: return None # Discard boxes that are larger than 80% of the image width OR height if discard_before_truncation: img_box_w = img_box[2] - img_box[0] img_box_h = img_box[3] - img_box[1] if img_box_w > (image_w * 0.8) or img_box_h > (image_h * 0.8): return None # Truncate remaining boxes into image space if img_box[0] < 0: img_box[0] = 0 if img_box[1] < 0: img_box[1] = 0 if img_box[2] > image_w: img_box[2] = image_w if img_box[3] > image_h: img_box[3] = image_h # Discard boxes that are covering the the whole image after truncation if not discard_before_truncation: img_box_w = img_box[2] - img_box[0] img_box_h = img_box[3] - img_box[1] if img_box_w > (image_w * 0.8) and img_box_h > (image_h * 0.8): return None return img_box
def project_to_bev(boxes_3d, bev_extents): """ Projects an array of 3D boxes into bird's eye view Args: boxes_3d: list of 3d boxes in the format: N x [x, y, z, l, w, h, ry] bev_extents: xz extents of the 3d area [[min_x, max_x], [min_z, max_z]] Returns: box_points: counter-clockwise order box points in bev map space N x [[x0, y0], ... [x3, y3]] - (N x 4 x 2) box_points_norm: points normalized as a percentage of the map size N x [[x0, y0], ... [x3, y3]] - (N x 4 x 2) """ format_checker.check_box_3d_format(boxes_3d) boxes_3d = np.array(boxes_3d, dtype=np.float32) x = boxes_3d[:, 0] z = boxes_3d[:, 2] l = boxes_3d[:, 3] w = boxes_3d[:, 4] ry = boxes_3d[:, 6] # 1|0 2D corners # 2|3 l_2 = l / 2.0 w_2 = w / 2.0 p0 = np.array([l_2, w_2]) p1 = np.array([-l_2, w_2]) p2 = np.array([-l_2, -w_2]) p3 = np.array([l_2, -w_2]) box_points = np.empty((len(boxes_3d), 4, 2)) for box_idx in range(len(boxes_3d)): rot = ry[box_idx] rot_mat = np.reshape([[np.cos(rot), np.sin(rot)], [-np.sin(rot), np.cos(rot)]], (2, 2)) box_x = x[box_idx] box_z = z[box_idx] box_xz = [box_x, box_z] box_p0 = np.dot(rot_mat, p0[:, box_idx]) + box_xz box_p1 = np.dot(rot_mat, p1[:, box_idx]) + box_xz box_p2 = np.dot(rot_mat, p2[:, box_idx]) + box_xz box_p3 = np.dot(rot_mat, p3[:, box_idx]) + box_xz box_points[box_idx] = np.array([box_p0, box_p1, box_p2, box_p3]) # Calculate normalized box corners for ROI pooling x_extents_min = bev_extents[0][0] z_extents_min = bev_extents[1][1] # z axis is reversed points_shifted = box_points - [x_extents_min, z_extents_min] x_extents_range = bev_extents[0][1] - bev_extents[0][0] z_extents_range = bev_extents[1][0] - bev_extents[1][1] box_points_norm = points_shifted / [x_extents_range, z_extents_range] box_points = np.asarray(box_points, dtype=np.float32) box_points_norm = np.asarray(box_points_norm, dtype=np.float32) return box_points, box_points_norm
def project_to_bev_box(boxes_3d, bev_extents): """ Projects an array of 3D boxes into bird's eye view Args: boxes_3d: list of 3d boxes in the format: N x [x, y, z, l, w, h, ry] bev_extents: xz extents of the 3d area [[min_x, max_x], [min_z, max_z]] Returns: box: rotated box(x_center, y_center, x_dimension, y_dimension, ry) N x [x_bev, y_bev, w_bev, h_bev, ry] - (N x 5) box_points_norm: points normalized as a percentage of the map size N x [[x0, y0], ... [x3, y3]] - (N x 4 x 2) """ format_checker.check_box_3d_format(boxes_3d) boxes_3d = np.array(boxes_3d, dtype=np.float32) x = boxes_3d[:, 0] z = boxes_3d[:, 2] l = boxes_3d[:, 3] w = boxes_3d[:, 4] ry = boxes_3d[:, 6] l_2 = l / 2.0 w_2 = w / 2.0 p0 = np.array([l_2, w_2]) p1 = np.array([-l_2, w_2]) p2 = np.array([-l_2, -w_2]) p3 = np.array([l_2, -w_2]) box_points = np.empty((len(boxes_3d), 4, 2)) for box_idx in range(len(boxes_3d)): rot = ry[box_idx] rot_mat = np.reshape([[np.cos(rot), np.sin(rot)], [-np.sin(rot), np.cos(rot)]], (2, 2)) box_x = x[box_idx] box_z = z[box_idx] box_xz = [box_x, box_z] box_p0 = np.dot(rot_mat, p0[:, box_idx]) + box_xz box_p1 = np.dot(rot_mat, p1[:, box_idx]) + box_xz box_p2 = np.dot(rot_mat, p2[:, box_idx]) + box_xz box_p3 = np.dot(rot_mat, p3[:, box_idx]) + box_xz box_points[box_idx] = np.array([box_p0, box_p1, box_p2, box_p3]) # Calculate normalized box corners for ROI pooling x_extents_min = bev_extents[0][0] z_extents_min = bev_extents[1][1] # z axis is reversed points_shifted = box_points - [x_extents_min, z_extents_min] x_extents_range = bev_extents[0][1] - bev_extents[0][0] z_extents_range = bev_extents[1][0] - bev_extents[1][1] #bev image_x_axis = 3D x_axis, bev image_y_axis = -3D z_axis (y is down, y=0 corresponds to z=max_z) box_points_norm = points_shifted / [x_extents_range, z_extents_range] box_points_norm = np.asarray(box_points_norm, dtype=np.float32) box = np.vstack([x, z, l, w, ry]).transpose() box_norm = box.copy() box_shifted = box_norm[:, :2] - [x_extents_min, z_extents_min] box_norm[:, :2] = box_shifted / [x_extents_range, z_extents_range] #size normalize, dont have to shift. #[WRONG!]box_norm[:, 2:4] = box_norm[:, 2:4] / [x_extents_range, z_extents_range] box_norm[:, 2] = np.sqrt((box_points_norm[:, 0, 0] - box_points_norm[:, 1, 0])**2 + \ (box_points_norm[:, 0, 1] - box_points_norm[:, 1, 1]) **2 ) box_norm[:, 3] = np.sqrt((box_points_norm[:, 1, 0] - box_points_norm[:, 2, 0])**2 + \ (box_points_norm[:, 1, 1] - box_points_norm[:, 2, 1]) **2 ) #box_dir = box_points_norm[:, 1] - box_points_norm[:, 0] #box_norm[:, -1] = np.arctan2(box_dir[:, 1], box_dir[:, 0]) #rotate to corner first and then normalize != normalize first and then rotate to corner box_norm = np.asarray(box_norm, dtype=np.float32) #use to do transformation between BEV and 3D. #norm_factor={'xmin':x_extents_min, 'zmin':z_extents_min, 'xrange':x_extents_range, 'zrange':z_extents_range} #return box_norm, box_points_norm, norm_factor return box_norm, box_points_norm
def tf_box_3d_to_box_4c(boxes_3d, ground_plane): """Vectorized conversion of box_3d to box_4c tensors Args: boxes_3d: Tensor of boxes_3d (N, 7) ground_plane: Tensor ground plane coefficients (4,) Returns: Tensor of boxes_4c (N, 10) """ format_checker.check_box_3d_format(boxes_3d) anchors = box_3d_encoder.tf_box_3d_to_anchor(boxes_3d) centroid_x = anchors[:, 0] centroid_y = anchors[:, 1] centroid_z = anchors[:, 2] dim_x = anchors[:, 3] dim_y = anchors[:, 4] dim_z = anchors[:, 5] # Create temporary box at (0, 0) for rotation half_dim_x = dim_x / 2 half_dim_z = dim_z / 2 # Box corners x_corners = tf.stack([half_dim_x, half_dim_x, -half_dim_x, -half_dim_x], axis=1) z_corners = tf.stack([half_dim_z, -half_dim_z, -half_dim_z, half_dim_z], axis=1) # Rotations from boxes_3d all_rys = boxes_3d[:, 6] # Find nearest 90 degree half_pi = np.pi / 2 ortho_rys = tf.round(all_rys / half_pi) * half_pi # Get rys and 0/1 padding ry_diffs = all_rys - ortho_rys zeros = tf.zeros_like(ry_diffs, dtype=tf.float32) ones = tf.ones_like(ry_diffs, dtype=tf.float32) # Create transformation matrix, including rotation and translation tr_mat = tf.stack( [tf.stack([tf.cos(ry_diffs), tf.sin(ry_diffs), centroid_x], axis=1), tf.stack([-tf.sin(ry_diffs), tf.cos(ry_diffs), centroid_z], axis=1), tf.stack([zeros, zeros, ones], axis=1)], axis=2) # Create a ones row ones_row = tf.ones_like(x_corners) # Append the column of ones to be able to multiply points_stacked = tf.stack([x_corners, z_corners, ones_row], axis=1) corners = tf.matmul(tr_mat, points_stacked, transpose_a=True, transpose_b=False) # Discard the last row (ones) corners = corners[:, 0:2] flat_corners = tf.reshape(corners, [-1, 8]) # Get ground plane coefficients a = ground_plane[0] b = ground_plane[1] c = ground_plane[2] d = ground_plane[3] # Calculate heights off ground plane ground_y = -(a * centroid_x + c * centroid_z + d) / b h1 = ground_y - centroid_y h2 = h1 + dim_y batched_h1 = tf.reshape(h1, [-1, 1]) batched_h2 = tf.reshape(h2, [-1, 1]) # Stack into (?, 10) box_4c = tf.concat([flat_corners, batched_h1, batched_h2], axis=1) return box_4c
def np_box_3d_to_box_4c(box_3d, ground_plane): """Converts a single box_3d to box_4c Args: box_3d: box_3d (6,) ground_plane: ground plane coefficients (4,) Returns: box_4c (10,) """ format_checker.check_box_3d_format(box_3d) anchor = box_3d_encoder.box_3d_to_anchor(box_3d, ortho_rotate=True)[0] centroid_x = anchor[0] centroid_y = anchor[1] centroid_z = anchor[2] dim_x = anchor[3] dim_y = anchor[4] dim_z = anchor[5] # Create temporary box at (0, 0) for rotation half_dim_x = dim_x / 2 half_dim_z = dim_z / 2 # Box corners x_corners = np.asarray([half_dim_x, half_dim_x, -half_dim_x, -half_dim_x]) z_corners = np.array([half_dim_z, -half_dim_z, -half_dim_z, half_dim_z]) ry = box_3d[6] # Find nearest 90 degree half_pi = np.pi / 2 ortho_ry = np.round(ry / half_pi) * half_pi # Find rotation to make the box ortho aligned ry_diff = ry - ortho_ry # Create transformation matrix, including rotation and translation tr_mat = np.array([[np.cos(ry_diff), np.sin(ry_diff), centroid_x], [-np.sin(ry_diff), np.cos(ry_diff), centroid_z], [0, 0, 1]]) # Create a ones row ones_row = np.ones(x_corners.shape) # Append the column of ones to be able to multiply points_stacked = np.vstack([x_corners, z_corners, ones_row]) corners = np.matmul(tr_mat, points_stacked) # Discard the last row (ones) corners = corners[0:2] # Calculate height off ground plane ground_y = geometry_utils.calculate_plane_point( ground_plane, [centroid_x, None, centroid_z])[1] h1 = ground_y - centroid_y h2 = h1 + dim_y # Stack into (10,) ndarray box_4c = np.hstack([corners.flatten(), h1, h2]) return box_4c
def tf_box_3d_to_box_8co(boxes_3d): """Computes the 3D bounding box corner positions from Box3D format. The order of corners are preserved during this conversion. Args: boxes_3d: N x 7 tensor of box_3d in the format [x, y, z, l, w, h, ry] Returns: corners_3d: An ndarray or a tensor of shape (N x 3 x 8) representing the box as corners in following format -> [[[x1,...,x8],[y1...,y8], [z1,...,z8]]]. """ format_checker.check_box_3d_format(boxes_3d) all_rys = boxes_3d[:, 6] ry_sin = tf.sin(all_rys) ry_cos = tf.cos(all_rys) zeros = tf.zeros_like(all_rys, dtype=tf.float32) ones = tf.ones_like(all_rys, dtype=tf.float32) # Rotation matrix rot_mats = tf.stack([ tf.stack([ry_cos, zeros, ry_sin], axis=1), tf.stack([zeros, ones, zeros], axis=1), tf.stack([-ry_sin, zeros, ry_cos], axis=1) ], axis=2) length = boxes_3d[:, 3] width = boxes_3d[:, 4] height = boxes_3d[:, 5] half_length = length / 2 half_width = width / 2 x_corners = tf.stack([ half_length, half_length, -half_length, -half_length, half_length, half_length, -half_length, -half_length ], axis=1) y_corners = tf.stack( [zeros, zeros, zeros, zeros, -height, -height, -height, -height], axis=1) z_corners = tf.stack([ half_width, -half_width, -half_width, half_width, half_width, -half_width, -half_width, half_width ], axis=1) corners = tf.stack([x_corners, y_corners, z_corners], axis=1) boxes_8c = tf.matmul(rot_mats, corners, transpose_a=True, transpose_b=False) # Translate the corners corners_3d_x = boxes_8c[:, 0] + tf.reshape(boxes_3d[:, 0], (-1, 1)) corners_3d_y = boxes_8c[:, 1] + tf.reshape(boxes_3d[:, 1], (-1, 1)) corners_3d_z = boxes_8c[:, 2] + tf.reshape(boxes_3d[:, 2], (-1, 1)) boxes_8c = tf.stack([corners_3d_x, corners_3d_y, corners_3d_z], axis=1) return boxes_8c
def tf_box_3d_to_box_8c(boxes_3d): """Computes the 3D bounding box corner positions from box_3d format. This function does not preserve corners order during conversion from box_3d -> box_8c. Instead of using the box_3d's orientation, 'ry', nearest 90 degree angle is selected to create an axis-aligned box. This helps in calculating the closest corner to corner when comparing the corners to the ground-truth boxes. Args: boxes_3d: N x 7 tensor of box_3d in the format [x, y, z, l, w, h, ry] Returns: corners_3d: A tensor of shape (N x 3 x 8) representing the box as corners in following format -> [[[x1,...,x8],[y1...,y8], [z1,...,z8]]]. """ format_checker.check_box_3d_format(boxes_3d) anchors = box_3d_encoder.tf_box_3d_to_anchor(boxes_3d) centroid_x = anchors[:, 0] centroid_y = anchors[:, 1] centroid_z = anchors[:, 2] dim_x = anchors[:, 3] dim_y = anchors[:, 4] dim_z = anchors[:, 5] all_rys = boxes_3d[:, 6] # Find nearest 90 degree half_pi = np.pi / 2 ortho_rys = tf.round(all_rys / half_pi) * half_pi ry_diff = all_rys - ortho_rys ry_sin = tf.sin(ry_diff) ry_cos = tf.cos(ry_diff) zeros = tf.zeros_like(ry_diff, dtype=tf.float32) ones = tf.ones_like(ry_diff, dtype=tf.float32) # Rotation matrix rot_mats = tf.stack([ tf.stack([ry_cos, zeros, ry_sin], axis=1), tf.stack([zeros, ones, zeros], axis=1), tf.stack([-ry_sin, zeros, ry_cos], axis=1) ], axis=2) half_dim_x = dim_x / 2 half_dim_z = dim_z / 2 x_corners = tf.stack([ half_dim_x, half_dim_x, -half_dim_x, -half_dim_x, half_dim_x, half_dim_x, -half_dim_x, -half_dim_x ], axis=1) y_corners = tf.stack( [zeros, zeros, zeros, zeros, -dim_y, -dim_y, -dim_y, -dim_y], axis=1) z_corners = tf.stack([ half_dim_z, -half_dim_z, -half_dim_z, half_dim_z, half_dim_z, -half_dim_z, -half_dim_z, half_dim_z ], axis=1) corners = tf.stack([x_corners, y_corners, z_corners], axis=1) boxes_8c = tf.matmul(rot_mats, corners, transpose_a=True, transpose_b=False) # Translate the corners corners_3d_x = boxes_8c[:, 0] + tf.reshape(centroid_x, (-1, 1)) corners_3d_y = boxes_8c[:, 1] + tf.reshape(centroid_y, (-1, 1)) corners_3d_z = boxes_8c[:, 2] + tf.reshape(centroid_z, (-1, 1)) boxes_8c = tf.stack([corners_3d_x, corners_3d_y, corners_3d_z], axis=1) return boxes_8c
def np_box_3d_to_box_8c(box_3d): """Computes the 3D bounding box corner positions from box_3d format. This function does not preserve corners order but rather the corners are rotated to the nearest 90 degree angle. This helps in calculating the closest corner to corner when comparing the corners to the ground- truth boxes. Args: box_3d: ndarray of size (7,) representing box_3d in the format [x, y, z, l, w, h, ry] Returns: corners_3d: An ndarray or a tensor of shape (3 x 8) representing the box as corners in following format -> [[x1,...,x8],[y1...,y8], [z1,...,z8]]. """ format_checker.check_box_3d_format(box_3d) # This function is vectorized and returns an ndarray anchor = box_3d_encoder.box_3d_to_anchor(box_3d, ortho_rotate=True)[0] centroid_x = anchor[0] centroid_y = anchor[1] centroid_z = anchor[2] dim_x = anchor[3] dim_y = anchor[4] dim_z = anchor[5] half_dim_x = dim_x / 2 half_dim_z = dim_z / 2 # 3D BB corners x_corners = np.array([ half_dim_x, half_dim_x, -half_dim_x, -half_dim_x, half_dim_x, half_dim_x, -half_dim_x, -half_dim_x ]) y_corners = np.array([0.0, 0.0, 0.0, 0.0, -dim_y, -dim_y, -dim_y, -dim_y]) z_corners = np.array([ half_dim_z, -half_dim_z, -half_dim_z, half_dim_z, half_dim_z, -half_dim_z, -half_dim_z, half_dim_z ]) ry = box_3d[6] # Find nearest 90 degree half_pi = np.pi / 2 ortho_ry = np.round(ry / half_pi) * half_pi # Find rotation to make the box ortho aligned ry_diff = ry - ortho_ry # Compute transform matrix # This includes rotation and translation rot = np.array([[np.cos(ry_diff), 0, np.sin(ry_diff), centroid_x], [0, 1, 0, centroid_y], [-np.sin(ry_diff), 0, np.cos(ry_diff), centroid_z]]) # Create a ones column ones_col = np.ones(x_corners.shape) # Append the column of ones to be able to multiply box_8c = np.dot(rot, np.array([x_corners, y_corners, z_corners, ones_col])) # Ignore the fourth column box_8c = box_8c[0:3] return box_8c