def visualize_bounding_box_camera_coordinates(P, points_3d_w, img):
"""
Visualize a bounding box over the box-like item in the image.
Args:
- P: 3x4 camera projection matrix
- points_3d_w : 8 x 4 array of points [X_i,Y_i,Z_i,1] in homogenouos coordinates
or 8 x 3 array of points [X_i,Y_i,Z_i], which should be the
coordinates of the bounding box's eight vertices in world
coordinate system.
- img: A numpy array, which should be the image in which we are going to
visualize the bounding box.
"""
# find K and cRw from P
K, wRc_T = decompose_camera_matrix(P)
M = np.matmul(np.linalg.inv(K), P)
M = np.concatenate([M, np.array([[0, 0, 0, 1]])], axis=0)
# transform the vertices to camera coordinates
points_3d_c = convert_3d_points_to_camera_coordinate(M, points_3d_w)
# project to 2D
projected = projection_from_camera_coordinates(K, points_3d_c)
# load and show the image
_, ax = plt.subplots()
ax.imshow(img)
units = [np.array([1, 0, 0]), np.array([0, 1, 0]), np.array([0, 0, 1])]
# draw the bounding box
for i, j in itertools.combinations(range(len(points_3d_w)), 2):
d = points_3d_w[i, :] - points_3d_w[j, :]
mod = np.dot(d, d)
if any(np.square(np.dot(d, unit)) == mod for unit in units):
ax.plot((projected[i, 0], projected[j, 0]),
(projected[i, 1], projected[j, 1]),
'-',
c='green')
def test_decompose_camera_matrix():
'''
tests whether projection was implemented correctly
'''
test_input = np.array(
[[122.43413524, -58.4445669, -8.71785439, 1637.28675475],
[4.54429487, 3.30940264, -134.40907701, 2880.869899],
[0.02429085, 0.02388273, -0.01160657, 1.]])
test_R = np.array([[0.70259051, -0.71156708, 0.00623408],
[-0.22535139, -0.23080127, -0.94654505],
[0.67496913, 0.66362871, -0.32251142]])
test_K = np.array([[127.55394371, -5.84978098, 46.66537651],
[0., 125.43636838, 48.61193508], [0., 0., 0.03598809]])
K, R = decompose_camera_matrix(test_input)
assert K.shape == test_K.shape and R.shape == test_R.shape
assert np.allclose(test_R, R, atol=1e-8)
assert np.allclose(test_K, K, atol=1e-8)