def draw( self, img, rvec, tvec, status, cameraMatrix, distCoeffs, triangl_idxs, nontriangl_idxs, all_idxs_tmp, new_imgp, imgp_to_objp_idxs, objp, objp_groups, group_id, color_palette, color_palette_size): # TODO: move these variables to another class """ Draw 2D composite view. 'status' can have the following values: 0: bad frame 1: good frame but not a keyframe 2: good frame and also a keyframe """ # Start drawing on copy of current image self.img[:, :, :] = img if status: # Draw world axis-system draw_axis_system(self.img, rvec, tvec, cameraMatrix, distCoeffs) # Draw to-be-triangulated point as dots with text indicating depth w.r.t. the camera imgp = idxs_get_new_imgp_by_idxs(triangl_idxs, new_imgp, all_idxs_tmp) text_imgp = np.array(imgp) text_imgp += [(-15, 10)] # adjust for text-position objp_idxs = imgp_to_objp_idxs[np.array(sorted(triangl_idxs))] groups = objp_groups[objp_idxs] P = trfm.P_from_R_and_t(cvh.Rodrigues(rvec), tvec) objp_depth = trfm.projection_depth(objp[objp_idxs], P) objp_colors = color_palette[ groups % color_palette_size] # set color by group id for ip, ipt, opd, opg, color in zip(imgp, text_imgp, objp_depth, groups, objp_colors): cvh.circle(self.img, ip, 2, color, thickness=-1) # draw triangulated point cvh.putText(self.img, "%.3f" % opd, ipt, fontFace, fontScale, color) # draw depths # Draw to-be-triangulated point as a cross nontriangl_imgp = idxs_get_new_imgp_by_idxs( nontriangl_idxs, new_imgp, all_idxs_tmp).astype(int) color = color_palette[group_id % color_palette_size] for p in nontriangl_imgp: cvh.line(self.img, (p[0] - 2, p[1]), (p[0] + 2, p[1]), color) cvh.line(self.img, (p[0], p[1] - 2), (p[0], p[1] + 2), color) else: # Draw red corner around image: it's a bad frame for (p1, p2) in self.image_box: cvh.line(self.img, p1, p2, rgb(255, 0, 0), thickness=4) # Display image cv2.imshow(self.img_title, self.img) cv2.waitKey()
def draw(self, img, rvec, tvec, status, cameraMatrix, distCoeffs, triangl_idxs, nontriangl_idxs, all_idxs_tmp, new_imgp, imgp_to_objp_idxs, objp, objp_groups, group_id, color_palette, color_palette_size): # TODO: move these variables to another class """ Draw 2D composite view. 'status' can have the following values: 0: bad frame 1: good frame but not a keyframe 2: good frame and also a keyframe """ # Start drawing on copy of current image self.img[:, :, :] = img if status: # Draw world axis-system draw_axis_system(self.img, rvec, tvec, cameraMatrix, distCoeffs) # Draw to-be-triangulated point as dots with text indicating depth w.r.t. the camera imgp = idxs_get_new_imgp_by_idxs(triangl_idxs, new_imgp, all_idxs_tmp) text_imgp = np.array(imgp) text_imgp += [(-15, 10)] # adjust for text-position objp_idxs = imgp_to_objp_idxs[np.array(sorted(triangl_idxs))] groups = objp_groups[objp_idxs] P = trfm.P_from_R_and_t(cvh.Rodrigues(rvec), tvec) objp_depth = trfm.projection_depth(objp[objp_idxs], P) objp_colors = color_palette[groups % color_palette_size] # set color by group id for ip, ipt, opd, opg, color in zip(imgp, text_imgp, objp_depth, groups, objp_colors): cvh.circle(self.img, ip, 2, color, thickness=-1) # draw triangulated point cvh.putText(self.img, "%.3f" % opd, ipt, fontFace, fontScale, color) # draw depths # Draw to-be-triangulated point as a cross nontriangl_imgp = idxs_get_new_imgp_by_idxs(nontriangl_idxs, new_imgp, all_idxs_tmp).astype(int) color = color_palette[group_id % color_palette_size] for p in nontriangl_imgp: cvh.line(self.img, (p[0]-2, p[1]), (p[0]+2, p[1]), color) cvh.line(self.img, (p[0], p[1]-2), (p[0], p[1]+2), color) else: # Draw red corner around image: it's a bad frame for (p1, p2) in self.image_box: cvh.line(self.img, p1, p2, rgb(255,0,0), thickness=4) # Display image cv2.imshow(self.img_title, self.img) cv2.waitKey()
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