def draw(self, rvec, tvec, status,
triangl_idxs, imgp_to_objp_idxs, objp, objp_groups, color_palette, color_palette_size): # TODO: move these variables to another class
"""
Draw 3D composite view.
Navigate using the following keys:
LEFT/RIGHT UP/DOWN PAGEUP/PAGEDOWN HOME/END
'status' can have the following values:
0: bad frame
1: good frame but not a keyframe
2: good frame and also a keyframe
"""
# Calculate current camera's axis-system expressed in world coordinates and cache center
if status:
R_cam = cvh.Rodrigues(rvec)
cam_axissys_objp = np.empty((4, 3))
cam_axissys_objp[0, :] = -R_cam.T.dot(tvec).reshape(1, 3) # cam_origin
cam_axissys_objp[1:4, :] = cam_axissys_objp[0, :] + R_cam # cam_x, cam_y, cam_z
self.cams_pos = np.concatenate((self.cams_pos, cam_axissys_objp[0:1, :])) # cache cam_origin
if status == 2: # frame is a keyframe
self.cams_pos_keyfr = np.concatenate((self.cams_pos_keyfr, cam_axissys_objp[0:1, :])) # cache cam_origin
while True:
# Fill with dark gray background color
self.img.fill(56)
# Draw world axis system
if trfm.projection_depth(np.array([[0,0,4]]), self.P)[0] > 0: # only draw axis-system if its Z-axis is entirely in sight
draw_axis_system(self.img, cvh.Rodrigues(self.P[0:3, 0:3]), self.P[0:3, 3], self.K, None)
# Draw 3D points
objp_proj, objp_visible = trfm.project_points(objp, self.K, self.img.shape, self.P)
objp_visible = set(np.where(objp_visible)[0])
current_idxs = set(imgp_to_objp_idxs[np.array(tuple(triangl_idxs))]) & objp_visible
done_idxs = np.array(tuple(objp_visible - current_idxs), dtype=int)
current_idxs = np.array(tuple(current_idxs), dtype=int)
groups = objp_groups[current_idxs]
for opp, opg, color in zip(objp_proj[current_idxs], groups, color_palette[groups % color_palette_size]):
cvh.circle(self.img, opp[0:2], 4, color, thickness=-1) # draw point, big radius
groups = objp_groups[done_idxs]
for opp, opg, color in zip(objp_proj[done_idxs], groups, color_palette[groups % color_palette_size]):
cvh.circle(self.img, opp[0:2], 2, color, thickness=-1) # draw point, small radius
# Draw camera trajectory
cams_pos_proj, cams_pos_visible = trfm.project_points(self.cams_pos, self.K, self.img.shape, self.P)
cams_pos_proj = cams_pos_proj[np.where(cams_pos_visible)[0]]
color = rgb(0,0,0)
for p1, p2 in zip(cams_pos_proj[:-1], cams_pos_proj[1:]):
cvh.line(self.img, p1, p2, color, thickness=2) # interconnect trajectory points
cams_pos_keyfr_proj, cams_pos_keyfr_visible = trfm.project_points(self.cams_pos_keyfr, self.K, self.img.shape, self.P)
cams_pos_keyfr_proj = cams_pos_keyfr_proj[np.where(cams_pos_keyfr_visible)[0]]
color = rgb(255,255,255)
for p in cams_pos_proj:
cvh.circle(self.img, p, 1, color, thickness=-1) # draw trajectory points
for p in cams_pos_keyfr_proj:
cvh.circle(self.img, p, 3, color) # highlight keyframe trajectory points
# Draw current camera axis system
if status:
(cam_origin, cam_xAxis, cam_yAxis, cam_zAxis), cam_visible = \
trfm.project_points(cam_axissys_objp, self.K, self.img.shape, self.P)
if cam_visible.sum() == len(cam_visible): # only draw axis-system if it's entirely in sight
cvh.line(self.img, cam_origin, cam_xAxis, rgb(255,0,0), lineType=cv2.CV_AA)
cvh.line(self.img, cam_origin, cam_yAxis, rgb(0,255,0), lineType=cv2.CV_AA)
cvh.line(self.img, cam_origin, cam_zAxis, rgb(0,0,255), lineType=cv2.CV_AA)
cvh.circle(self.img, cam_zAxis, 3, rgb(0,0,255)) # small dot to highlight cam Z axis
else:
last_cam_origin, last_cam_visible = trfm.project_points(self.cams_pos[-1:], self.K, self.img.shape, self.P)
if last_cam_visible[0]: # only draw if in sight
cvh.putText(self.img, '?', last_cam_origin[0] - (11, 11), fontFace, fontScale * 4, rgb(255,0,0)) # draw '?' because it's a bad frame
# Display image
cv2.imshow(self.img_title, self.img)
# Translate view by keyboard
key = cv2.waitKey() & 0xFF
delta_t_view = np.zeros((3))
if key in (0x51, 0x53): # LEFT/RIGHT key
delta_t_view[0] = 2 * (key == 0x51) - 1 # LEFT -> increase cam X pos
elif key in (0x52, 0x54): # UP/DOWN key
delta_t_view[1] = 2 * (key == 0x52) - 1 # UP -> increase cam Y pos
elif key in (0x55, 0x56): # PAGEUP/PAGEDOWN key
delta_t_view[2] = 2 * (key == 0x55) - 1 # PAGEUP -> increase cam Z pos
elif key in (0x50, 0x57): # HOME/END key
delta_z_rot = 2 * (key == 0x50) - 1 # HOME -> counter-clockwise rotate around cam Z axis
self.P[0:3, 0:4] = cvh.Rodrigues((0, 0, delta_z_rot * pi/36)).dot(self.P[0:3, 0:4]) # by steps of 5 degrees
else:
break
self.P[0:3, 3] += delta_t_view * 0.3
def draw(
self, rvec, tvec, status, triangl_idxs, imgp_to_objp_idxs, objp,
objp_groups, color_palette,
color_palette_size): # TODO: move these variables to another class
"""
Draw 3D composite view.
Navigate using the following keys:
LEFT/RIGHT UP/DOWN PAGEUP/PAGEDOWN HOME/END
'status' can have the following values:
0: bad frame
1: good frame but not a keyframe
2: good frame and also a keyframe
"""
# Calculate current camera's axis-system expressed in world coordinates and cache center
if status:
R_cam = cvh.Rodrigues(rvec)
cam_axissys_objp = np.empty((4, 3))
cam_axissys_objp[0, :] = -R_cam.T.dot(tvec).reshape(
1, 3) # cam_origin
cam_axissys_objp[
1:4, :] = cam_axissys_objp[0, :] + R_cam # cam_x, cam_y, cam_z
self.cams_pos = np.concatenate(
(self.cams_pos, cam_axissys_objp[0:1, :])) # cache cam_origin
if status == 2: # frame is a keyframe
self.cams_pos_keyfr = np.concatenate(
(self.cams_pos_keyfr,
cam_axissys_objp[0:1, :])) # cache cam_origin
while True:
# Fill with dark gray background color
self.img.fill(56)
# Draw world axis system
if trfm.projection_depth(
np.array([[0, 0, 4]]), self.P
)[0] > 0: # only draw axis-system if its Z-axis is entirely in sight
draw_axis_system(self.img, cvh.Rodrigues(self.P[0:3, 0:3]),
self.P[0:3, 3], self.K, None)
# Draw 3D points
objp_proj, objp_visible = trfm.project_points(
objp, self.K, self.img.shape, self.P)
objp_visible = set(np.where(objp_visible)[0])
current_idxs = set(imgp_to_objp_idxs[np.array(
tuple(triangl_idxs))]) & objp_visible
done_idxs = np.array(tuple(objp_visible - current_idxs), dtype=int)
current_idxs = np.array(tuple(current_idxs), dtype=int)
groups = objp_groups[current_idxs]
for opp, opg, color in zip(
objp_proj[current_idxs], groups,
color_palette[groups % color_palette_size]):
cvh.circle(self.img, opp[0:2], 4, color,
thickness=-1) # draw point, big radius
groups = objp_groups[done_idxs]
for opp, opg, color in zip(
objp_proj[done_idxs], groups,
color_palette[groups % color_palette_size]):
cvh.circle(self.img, opp[0:2], 2, color,
thickness=-1) # draw point, small radius
# Draw camera trajectory
cams_pos_proj, cams_pos_visible = trfm.project_points(
self.cams_pos, self.K, self.img.shape, self.P)
cams_pos_proj = cams_pos_proj[np.where(cams_pos_visible)[0]]
color = rgb(0, 0, 0)
for p1, p2 in zip(cams_pos_proj[:-1], cams_pos_proj[1:]):
cvh.line(self.img, p1, p2, color,
thickness=2) # interconnect trajectory points
cams_pos_keyfr_proj, cams_pos_keyfr_visible = trfm.project_points(
self.cams_pos_keyfr, self.K, self.img.shape, self.P)
cams_pos_keyfr_proj = cams_pos_keyfr_proj[np.where(
cams_pos_keyfr_visible)[0]]
color = rgb(255, 255, 255)
for p in cams_pos_proj:
cvh.circle(self.img, p, 1, color,
thickness=-1) # draw trajectory points
for p in cams_pos_keyfr_proj:
cvh.circle(self.img, p, 3,
color) # highlight keyframe trajectory points
# Draw current camera axis system
if status:
(cam_origin, cam_xAxis, cam_yAxis, cam_zAxis), cam_visible = \
trfm.project_points(cam_axissys_objp, self.K, self.img.shape, self.P)
if cam_visible.sum() == len(
cam_visible
): # only draw axis-system if it's entirely in sight
cvh.line(self.img,
cam_origin,
cam_xAxis,
rgb(255, 0, 0),
lineType=cv2.CV_AA)
cvh.line(self.img,
cam_origin,
cam_yAxis,
rgb(0, 255, 0),
lineType=cv2.CV_AA)
cvh.line(self.img,
cam_origin,
cam_zAxis,
rgb(0, 0, 255),
lineType=cv2.CV_AA)
cvh.circle(self.img, cam_zAxis, 3,
rgb(0, 0,
255)) # small dot to highlight cam Z axis
else:
last_cam_origin, last_cam_visible = trfm.project_points(
self.cams_pos[-1:], self.K, self.img.shape, self.P)
if last_cam_visible[0]: # only draw if in sight
cvh.putText(self.img, '?', last_cam_origin[0] - (11, 11),
fontFace, fontScale * 4,
rgb(255, 0,
0)) # draw '?' because it's a bad frame
# Display image
cv2.imshow(self.img_title, self.img)
# Translate view by keyboard
key = cv2.waitKey() & 0xFF
delta_t_view = np.zeros((3))
if key in (0x51, 0x53): # LEFT/RIGHT key
delta_t_view[0] = 2 * (
key == 0x51) - 1 # LEFT -> increase cam X pos
elif key in (0x52, 0x54): # UP/DOWN key
delta_t_view[1] = 2 * (key
== 0x52) - 1 # UP -> increase cam Y pos
elif key in (0x55, 0x56): # PAGEUP/PAGEDOWN key
delta_t_view[2] = 2 * (
key == 0x55) - 1 # PAGEUP -> increase cam Z pos
elif key in (0x50, 0x57): # HOME/END key
delta_z_rot = 2 * (
key == 0x50
) - 1 # HOME -> counter-clockwise rotate around cam Z axis
self.P[0:3,
0:4] = cvh.Rodrigues((0, 0, delta_z_rot * pi / 36)).dot(
self.P[0:3, 0:4]) # by steps of 5 degrees
else:
break
self.P[0:3, 3] += delta_t_view * 0.3
def drawCamera(img, cam_origin, cam_axes, K, P, neg_fy=False, scale_factor=0.07, draw_axes=True, draw_frustum=True):
"""
Draws a camera (X right, Y down) with 3D position "cam_origin" and local axes "cam_axes"
on "img" where the viewer cam has (3x3 matrix) intrinsics "K" and (3x4 matrix) pose "P".
"neg_fy" : set to True if the drawn cam has negative Y focal length, otherwise False
"scale_factor" : the size of the drawn cam
"draw_axes" : set to True to draw the axes of the to-be-drawn cam, otherwise False
"draw_frustum" : set to True to draw the frustrum of the to-be-drawn cam, otherwise False
"""
# Define local object-points
objp_to_project = np.array([ [ 0. , 0. , 0.], # cam origin
[ 1. , 0., 0.], # cam X-axis
[ 0. , 1., 0.], # cam Y-axis
[ 0. , 0., 1.], # cam Z-axis
[-0.5, -0.3, 1.], # frustum top-left
[ 0.5, -0.3, 1.], # frustum top-right
[ 0.5, 0.3, 1.], # frustum bottom-right
[-0.5, 0.3, 1.], # frustum bottom-left
[-0.3, -0.3, 1.], # cam up indication triangle left
[ 0.3, -0.3, 1.], # cam up indication triangle right
[ 0. , -0.6, 1.] ]) # cam up indication triangle top
# Keep size of camera constant by normalizing using the distance between cam origin and origin of visualizing cam P
objp_to_project *= cv2.norm(cam_origin.T + P[0:3, 0:3].T.dot(P[0:3, 3:4])) * scale_factor
# Negative focal length requires the cam's Y-axis to be flipped
if neg_fy:
objp_to_project[:, 1] *= -1
# Transform points in cam coords to world coords
objp_to_project = cam_origin + objp_to_project.dot(cam_axes)
# Project world coords to the viewer cam defined by (P, K)
objp_projected, cam_visible = trfm.project_points(objp_to_project, K, img.shape, P)
# Only draw axis-system if it's entirely in sight
if cam_visible.sum() == len(cam_visible):
cam_origin = objp_projected[0]
if draw_axes:
cam_xAxis, cam_yAxis, cam_zAxis = objp_projected[1:4]
line(img, cam_origin, cam_xAxis, rgb(255,0,0), lineType=cv2.CV_AA) # X-axis (red)
line(img, cam_origin, cam_yAxis, rgb(0,255,0), lineType=cv2.CV_AA) # Y-axis (green)
line(img, cam_origin, cam_zAxis, rgb(0,0,255), lineType=cv2.CV_AA) # Z-axis (blue)
circle(img, cam_zAxis, 3, rgb(0,0,255)) # small dot to highlight cam Z axis
if draw_frustum:
yellow = rgb(255,255,0)
frustum_plane = objp_projected[4:8]
for i, p in enumerate(frustum_plane):
# Create frustum plane
line(img, frustum_plane[i], frustum_plane[(i+1) % 4], yellow, lineType=cv2.CV_AA)
# Connect frustum plane points with origin
line(img, cam_origin, p, yellow, lineType=cv2.CV_AA)
# Draw up-facing frustum triangle
cv2.fillPoly(img, [objp_projected[8:11]], yellow, lineType=cv2.CV_AA)
return img
