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 calibrate_relative_poses_interactive(image_sets, cameraMatrixs, distCoeffss, imageSizes, boardSizes, board_scales, board_rvecs, board_tvecs): """ Make an estimate of the relative poses (as 4x4 projection matrices) between many cameras. Base these relative poses to the first camera. Each camera should be looking at its own chessboard, use different sizes of chessboards if a camera sees chessboard that are not associated with that camera. 'board_scales' scales the chessboard-units to world-units. 'board_rvecs' and 'board_tvecs' transform the rescaled local chessboard-coordinates to world-coordinates. The inverse of the reprojection error is used for weighting. """ num_cams = len(image_sets) num_images = len(image_sets[0]) reproj_error_max = 0 # Preprocess object-points of the different boards board_objps = [] for boardSize, board_scale, board_rvec, board_tvec in zip( boardSizes, board_scales, board_rvecs, board_tvecs): objp = np.ones((np.prod(boardSize), 4)) objp[:, 0:3] = calibration_tools.grid_objp(boardSize) * board_scale objp = objp.dot( trfm.P_from_R_and_t(cvh.Rodrigues(board_rvec), np.array(board_tvec).reshape(3, 1))[0:3, :].T) board_objps.append(objp) # Calculate all absolute poses Ps = np.zeros((num_images, num_cams, 4, 4)) weights = np.zeros((num_images, 1, 1, 1)) for i, images in enumerate(zip(*image_sets)): reproj_error = 0 for c, (image, cameraMatrix, distCoeffs, imageSize, boardSize, board_objp) in enumerate( zip(images, cameraMatrixs, distCoeffss, imageSizes, boardSizes, board_objps)): img = cv2.imread(image) ret, corners = cvh.extractChessboardFeatures(img, boardSize) if not ret: print( "Error: Image '%s' didn't contain a chessboard of size %s." % (image, boardSize)) return False, None # Draw and display the corners cv2.drawChessboardCorners(img, boardSize, corners, ret) cv2.imshow("img", img) cv2.waitKey(100) ret, rvec, tvec = cv2.solvePnP(board_objp, corners, cameraMatrix, distCoeffs) Ps[i, c, :, :] = trfm.P_from_R_and_t(cvh.Rodrigues(rvec), tvec) reproj_error = max( reprojection_error( # max: worst case [board_objp], [corners], cameraMatrix, distCoeffs, [rvec], [tvec])[1], reproj_error) reproj_error_max = max(reproj_error, reproj_error_max) weights[i] = 1. / reproj_error # Apply weighting on Ps, and rebase against first camera Ps *= weights / weights.sum() Ps = Ps.sum(axis=0) Pref_inv = trfm.P_inv(Ps[0, :, :]) # use first cam as reference return True, [P.dot(Pref_inv) for P in Ps], reproj_error_max
def triangl_pose_est_interactive(img_left, img_right, cameraMatrix, distCoeffs, imageSize, objp, boardSize, nonplanar_left, nonplanar_right): """ (TODO: remove debug-prints) Triangulation and relative pose estimation will be performed from LEFT to RIGHT image. Both images have to contain the whole chessboard, in order to make a decent estimation of the relative pose based on 'solvePnP'. Then the user can manually create matches between non-planar objects. If the user omits this step, only triangulation of the chessboard corners will be performed, and they will be compared to the real 3D points. Otherwise the coordinates of the triangulated points of the manually matched points will be printed, and a relative pose estimation will be performed (using the essential matrix), this pose estimation will be compared with the decent 'solvePnP' estimation. In that case you will be asked whether you want to mute the chessboard corners in all calculations, note that this requires at least 8 pairs of matches corresponding with non-planar geometry. During manual matching process, switching between LEFT and RIGHT image will be done in a zigzag fashion. To stop selecting matches, press SPACE. Additionally, you can also introduce predefined non-planar geometry, this is e.g. useful if you don't want to select non-planar geometry manually. """ ### Setup initial state, and calc some very accurate poses to compare against with later K_left = cameraMatrix K_right = cameraMatrix K_inv_left = cvh.invert(K_left) K_inv_right = cvh.invert(K_right) distCoeffs_left = distCoeffs distCoeffs_right = distCoeffs # Extract chessboard features, together they form a set of 'planar' points ret_left, planar_left = cvh.extractChessboardFeatures(img_left, boardSize) ret_right, planar_right = cvh.extractChessboardFeatures( img_right, boardSize) if not ret_left or not ret_right: print("Chessboard is not (entirely) in sight, aborting.") return # Save exact 2D and 3D points of the chessboard num_planar = planar_left.shape[0] planar_left_orig, planar_right_orig = planar_left, planar_right objp_orig = objp # Calculate P matrix of left pose, in reality this one is known ret, rvec_left, tvec_left = cv2.solvePnP(objp, planar_left, K_left, distCoeffs_left) P_left = trfm.P_from_R_and_t(cvh.Rodrigues(rvec_left), tvec_left) # Calculate P matrix of right pose, in reality this one is unknown ret, rvec_right, tvec_right = cv2.solvePnP(objp, planar_right, K_right, distCoeffs_right) P_right = trfm.P_from_R_and_t(cvh.Rodrigues(rvec_right), tvec_right) # Load previous non-planar inliers, if desired if nonplanar_left.size and not raw_input( "Type 'no' if you don't want to use previous non-planar inliers? " ).strip().lower() == "no": nonplanar_left = map(tuple, nonplanar_left) nonplanar_right = map(tuple, nonplanar_left) else: nonplanar_left = [] nonplanar_right = [] ### Create predefined non-planar 3D geometry, to enable automatic selection of non-planar points if raw_input( "Type 'yes' if you want to create and select predefined non-planar geometry? " ).strip().lower() == "yes": # A cube cube_coords = np.array([(0., 0., 0.), (1., 0., 0.), (1., 1., 0.), (0., 1., 0.), (0., 0., 1.), (1., 0., 1.), (1., 1., 1.), (0., 1., 1.)]) cube_coords *= 2 # scale cube_edges = np.array([(0, 1), (1, 2), (2, 3), (3, 0), (4, 5), (5, 6), (6, 7), (7, 4), (0, 4), (1, 5), (2, 6), (3, 7)]) # An 8-point circle s2 = 1. / np.sqrt(2) circle_coords = np.array([(1., 0., 0.), (s2, s2, 0.), (0., 1., 0.), (-s2, s2, 0.), (-1., 0., 0.), (-s2, -s2, 0.), (0., -1., 0.), (s2, -s2, 0.)]) circle_edges = np.array([(i, (i + 1) % 8) for i in range(8)]) # Position 2 cubes and 2 circles in the scene cube1 = np.array(cube_coords) cube1[:, 1] -= 1 cube2 = np.array(cube_coords) cube2 = cvh.Rodrigues((0., 0., pi / 4)).dot(cube2.T).T cube2[:, 0] += 4 cube2[:, 1] += 3 cube2[:, 2] += 2 circle1 = np.array(circle_coords) circle1 *= 2 circle1[:, 1] += 5 circle1[:, 2] += 2 circle2 = np.array(circle_coords) circle2 = cvh.Rodrigues((pi / 2, 0., 0.)).dot(circle2.T).T circle2[:, 1] += 5 circle2[:, 2] += 2 # Print output to be used in Blender (see "calibrate_pose_visualization.blend") print() print("Cubes") print("edges_cube = \\\n", map(list, cube_edges)) print("coords_cube1 = \\\n", map(list, cube1)) print("coords_cube2 = \\\n", map(list, cube2)) print() print("Circles") print("edges_circle = \\\n", map(list, circle_edges)) print("coords_circle1 = \\\n", map(list, circle1)) print("coords_circle2 = \\\n", map(list, circle2)) print() color = rgb(0, 200, 150) for verts, edges in zip( [cube1, cube2, circle1, circle2], [cube_edges, cube_edges, circle_edges, circle_edges]): out_left = cvh.wireframe3DGeometry(img_left, verts, edges, color, rvec_left, tvec_left, K_left, distCoeffs_left) out_right = cvh.wireframe3DGeometry(img_right, verts, edges, color, rvec_right, tvec_right, K_right, distCoeffs_right) valid_match_idxs = [ i for i, (pl, pr) in enumerate(zip(out_left, out_right)) if 0 <= min(pl[0], pr[0]) <= max(pl[0], pr[0]) < imageSize[0] and 0 <= min(pl[1], pr[1]) <= max(pl[1], pr[1]) < imageSize[1] ] nonplanar_left += map(tuple, out_left[valid_match_idxs]) # concatenate nonplanar_right += map(tuple, out_right[valid_match_idxs]) # concatenate nonplanar_left = np.rint(nonplanar_left).astype(int) nonplanar_right = np.rint(nonplanar_right).astype(int) ### User can manually create matches between non-planar objects class ManualMatcher: def __init__(self, window_name, images, points): self.window_name = window_name self.images = images self.points = points self.img_idx = 0 # 0: left; 1: right cv2.namedWindow(window_name) cv2.setMouseCallback(window_name, self.onMouse) def onMouse(self, event, x, y, flags, userdata): if event == cv2.EVENT_LBUTTONDOWN: self.points[self.img_idx].append((x, y)) elif event == cv2.EVENT_LBUTTONUP: # Switch images in a ping-pong fashion if len(self.points[0]) != len(self.points[1]): self.img_idx = 1 - self.img_idx def run(self): print("Select your matches. Press SPACE when done.") while True: img = cv2.drawKeypoints(self.images[self.img_idx], [ cv2.KeyPoint(p[0], p[1], 7.) for p in self.points[self.img_idx] ], color=rgb(0, 0, 255)) cv2.imshow(self.window_name, img) key = cv2.waitKey(50) & 0xFF if key == ord(' '): break # finish if SPACE is pressed num_points_diff = len(self.points[0]) - len(self.points[1]) if num_points_diff: del self.points[num_points_diff < 0][-abs(num_points_diff):] print("Selected", len(self.points[0]), "pairs of matches.") # Execute the manual matching nonplanar_left, nonplanar_right = list(nonplanar_left), list( nonplanar_right) ManualMatcher("Select match-points of non-planar objects", [img_left, img_right], [nonplanar_left, nonplanar_right]).run() num_nonplanar = len(nonplanar_left) has_nonplanar = (num_nonplanar > 0) if 0 < num_nonplanar < 8: print("Warning: you've selected less than 8 pairs of matches.") nonplanar_left = np.array(nonplanar_left).reshape(num_nonplanar, 2) nonplanar_right = np.array(nonplanar_right).reshape(num_nonplanar, 2) mute_chessboard_corners = False if num_nonplanar >= 8 and raw_input( "Type 'yes' if you want to exclude chessboard corners from calculations? " ).strip().lower() == "yes": mute_chessboard_corners = True num_planar = 0 planar_left = np.zeros((0, 2)) planar_right = np.zeros((0, 2)) objp = np.zeros((0, 3)) print("Chessboard corners muted.") has_prev_triangl_points = not mute_chessboard_corners # normally in this example, this should always be True, unless user forces False ### Undistort points => normalized coordinates allfeatures_left = np.concatenate((planar_left, nonplanar_left)) allfeatures_right = np.concatenate((planar_right, nonplanar_right)) allfeatures_nrm_left = cv2.undistortPoints(np.array([allfeatures_left]), K_left, distCoeffs_left)[0] allfeatures_nrm_right = cv2.undistortPoints(np.array([allfeatures_right]), K_right, distCoeffs_right)[0] planar_nrm_left, nonplanar_nrm_left = allfeatures_nrm_left[:planar_left.shape[ 0]], allfeatures_nrm_left[planar_left.shape[0]:] planar_nrm_right, nonplanar_nrm_right = allfeatures_nrm_right[:planar_right.shape[ 0]], allfeatures_nrm_right[planar_right.shape[0]:] ### Calculate relative pose using the essential matrix # Only do pose estimation if we've got 2D points of non-planar geometry if has_nonplanar: # Determine inliers by calculating the fundamental matrix on all points, # except when mute_chessboard_corners is True: only use nonplanar_nrm_left and nonplanar_nrm_right F, status = cv2.findFundamentalMat( allfeatures_nrm_left, allfeatures_nrm_right, cv2.FM_RANSAC, 0.006 * np.amax(allfeatures_nrm_left), 0.99) # threshold from [Snavely07 4.1] # OpenCV BUG: "status" matrix is not initialized with zeros in some cases, reproducable with 2 sets of 8 2D points equal to (0., 0.) # maybe because "_mask.create()" is not called: # https://github.com/Itseez/opencv/blob/7e2bb63378dafb90063af40caff20c363c8c9eaf/modules/calib3d/src/ptsetreg.cpp#L185 # Workaround to test on outliers: use "!= 1", instead of "== 0" print(status.T) inlier_idxs = np.where(status == 1)[0] print("Removed", allfeatures_nrm_left.shape[0] - inlier_idxs.shape[0], "outlier(s).") num_planar = np.where(inlier_idxs < num_planar)[0].shape[0] num_nonplanar = inlier_idxs.shape[0] - num_planar print("num chessboard inliers:", num_planar) allfeatures_left, allfeatures_right = allfeatures_left[ inlier_idxs], allfeatures_right[inlier_idxs] allfeatures_nrm_left, allfeatures_nrm_right = allfeatures_nrm_left[ inlier_idxs], allfeatures_nrm_right[inlier_idxs] if not mute_chessboard_corners: objp = objp[inlier_idxs[:num_planar]] planar_left, planar_right = allfeatures_left[: num_planar], allfeatures_right[: num_planar] planar_nrm_left, planar_nrm_right = allfeatures_nrm_left[: num_planar], allfeatures_nrm_right[: num_planar] nonplanar_nrm_left, nonplanar_nrm_right = allfeatures_nrm_left[ num_planar:], allfeatures_nrm_right[num_planar:] # Calculate first solution of relative pose if allfeatures_nrm_left.shape[0] >= 8: F, status = cv2.findFundamentalMat(allfeatures_nrm_left, allfeatures_nrm_right, cv2.FM_8POINT) print("F:") print(F) else: print( "Error: less than 8 pairs of inliers found, I can't perform the 8-point algorithm." ) #E = (K_right.T) .dot (F) .dot (K_left) # "Multiple View Geometry in CV" by Hartley&Zisserman (9.12) E = F # K = I because we already normalized the coordinates print("Correct determinant of essential matrix?", (abs(cv2.determinant(E)) <= 1e-7)) w, u, vt = cv2.SVDecomp(E, flags=cv2.SVD_MODIFY_A) print(w) if ((w[0] < w[1] and w[0] / w[1]) or (w[1] < w[0] and w[1] / w[0]) or 0) < 0.7: print( "Essential matrix' 'w' vector deviates too much from expected") W = np.array([ [0., -1., 0.], # Hartley&Zisserman (9.13) [1., 0., 0.], [0., 0., 1.] ]) R = (u).dot(W).dot(vt) # Hartley&Zisserman result 9.19 det_R = cv2.determinant(R) print("Coherent rotation?", (abs(det_R) - 1 <= 1e-7)) if det_R - ( -1 ) < 1e-7: # http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid # E *= -1: vt *= -1 # svd(-E) = u * w * (-v).T R *= -1 # => u * W * (-v).T = -R print("det(R) == -1, compensated.") t = u[:, 2:3] # Hartley&Zisserman result 9.19 P = trfm.P_from_R_and_t(R, t) # Select right solution where the (center of the) 3d points are/is in front of both cameras, # when has_prev_triangl_points == True, we can do it a lot faster. test_point = np.ones((4, 1)) # 3D point to test the solutions with if has_prev_triangl_points: print("Using advantage of already triangulated points' position") print(P) # Select the closest already triangulated cloudpoint idx to the center of the cloud center_of_mass = objp.sum(axis=0) / objp.shape[0] center_objp_idx = np.argmin( ((objp - center_of_mass)**2).sum(axis=1)) print("center_objp_idx:", center_objp_idx) # Select the corresponding image points center_imgp_left = planar_nrm_left[center_objp_idx] center_imgp_right = planar_nrm_right[center_objp_idx] # Select the corresponding 3D point test_point[0:3, :] = objp[center_objp_idx].reshape(3, 1) print("test_point:") print(test_point) test_point = P_left.dot( test_point ) # set the reference axis-system to the one of the left camera, note that are_points_in_front_of_left_camera is automatically True center_objp_triangl, triangl_status = iterative_LS_triangulation( center_imgp_left.reshape(1, 2), np.eye(4), center_imgp_right.reshape(1, 2), P) print("triangl_status:", triangl_status) if (center_objp_triangl).dot(test_point[0:3, 0:1]) < 0: P[0:3, 3:4] *= -1 # do a baseline reversal print(P, "fixed triangulation inversion") if P[0:3, :].dot(test_point)[ 2, 0] < 0: # are_points_in_front_of_right_camera is False P[0:3, 0:3] = (u).dot(W.T).dot( vt) # use the other solution of the twisted pair ... P[0:3, 3:4] *= -1 # ... and also do a baseline reversal print(P, "fixed camera projection inversion") elif num_nonplanar > 0: print( "Doing all ambiguity checks since there are no already triangulated points" ) print(P) for i in range(4): # check all 4 solutions objp_triangl, triangl_status = iterative_LS_triangulation( nonplanar_nrm_left, np.eye(4), nonplanar_nrm_right, P) print("triangl_status:", triangl_status) center_of_mass = objp_triangl.sum(axis=0) / len( objp_triangl ) # select the center of the triangulated cloudpoints test_point[0:3, :] = center_of_mass.reshape(3, 1) print("test_point:") print(trfm.P_inv(P_left).dot(test_point)) if np.eye(3, 4).dot(test_point)[2, 0] > 0 and P[0:3, :].dot( test_point )[2, 0] > 0: # are_points_in_front_of_cameras is True break if i % 2: P[0:3, 0:3] = (u).dot(W.T).dot( vt) # use the other solution of the twisted pair print(P, "using the other solution of the twisted pair") else: P[0:3, 3:4] *= -1 # do a baseline reversal print(P, "doing a baseline reversal") are_points_in_front_of_left_camera = (np.eye( 3, 4).dot(test_point)[2, 0] > 0) are_points_in_front_of_right_camera = (P[0:3, :].dot(test_point)[2, 0] > 0) print("are_points_in_front_of_cameras?", are_points_in_front_of_left_camera, are_points_in_front_of_right_camera) if not (are_points_in_front_of_left_camera and are_points_in_front_of_right_camera): print("No valid solution found!") P_left_result = trfm.P_inv(P).dot(P_right) P_right_result = P.dot(P_left) print("P_left") print(P_left) print("P_rel") print(P) print("P_right") print(P_right) print( "=> error:", reprojection_error([objp_orig] * 2, [planar_left_orig, planar_right_orig], cameraMatrix, distCoeffs, [rvec_left, rvec_right], [tvec_left, tvec_right])[1]) print("P_left_result") print(P_left_result) print("P_right_result") print(P_right_result) # We use "P_left" instead of "P_left_result" because the latter depends on the unknown "P_right" print( "=> error:", reprojection_error([objp_orig] * 2, [planar_left_orig, planar_right_orig], cameraMatrix, distCoeffs, [ cvh.Rodrigues(P_left[0:3, 0:3]), cvh.Rodrigues(P_right_result[0:3, 0:3]) ], [P_left[0:3, 3], P_right_result[0:3, 3]])[1]) ### Triangulate objp_result = np.zeros((0, 3)) if has_prev_triangl_points: # Do triangulation of all points # NOTICE: in a real case, we should only use not yet triangulated points that are in sight objp_result, triangl_status = iterative_LS_triangulation( allfeatures_nrm_left, P_left, allfeatures_nrm_right, P_right) print("triangl_status:", triangl_status) elif num_nonplanar > 0: # We already did the triangulation during the pose estimation, but we still need to backtransform them from the left camera axis-system objp_result = trfm.P_inv(P_left).dot( np.concatenate((objp_triangl.T, np.ones((1, len(objp_triangl)))))) objp_result = objp_result[0:3, :].T print(objp_triangl) print("objp:") print(objp) print( "=> error:", reprojection_error([objp_orig] * 2, [planar_left_orig, planar_right_orig], cameraMatrix, distCoeffs, [rvec_left, rvec_right], [tvec_left, tvec_right])[1]) print("objp_result of chessboard:") print(objp_result[:num_planar, :]) if has_nonplanar: print("objp_result of non-planar geometry:") print(objp_result[num_planar:, :]) if num_planar + num_nonplanar == 0: print("=> error: undefined") else: print( "=> error:", reprojection_error([objp_result] * 2, [allfeatures_left, allfeatures_right], cameraMatrix, distCoeffs, [rvec_left, rvec_right], [tvec_left, tvec_right])[1]) ### Print total combined reprojection error # We only have both pose estimation and triangulation if we've got 2D points of non-planar geometry if has_nonplanar: if num_planar + num_nonplanar == 0: print("=> error: undefined") else: # We use "P_left" instead of "P_left_result" because the latter depends on the unknown "P_right" print( "Total combined error:", reprojection_error( [objp_result] * 2, [allfeatures_left, allfeatures_right], cameraMatrix, distCoeffs, [ cvh.Rodrigues(P_left[0:3, 0:3]), cvh.Rodrigues(P_right_result[0:3, 0:3]) ], [P_left[0:3, 3], P_right_result[0:3, 3]])[1]) """ Further things we can do (in a real case): 1. solvePnP() on inliers (including previously already triangulated points), or should we use solvePnPRansac() (in that case what to do with the outliers)? 2. re-triangulate on new pose estimation """ ### Print summary to be used in Blender to visualize (see "calibrate_pose_visualization.blend") print("Camera poses:") if has_nonplanar: print("Left") print_pose(rvec_left, tvec_left) print() print("Left_result") print_pose(cvh.Rodrigues(P_left_result[0:3, 0:3]), P_left_result[0:3, 3:4]) print() print("Right") print_pose(rvec_right, tvec_right) print() print("Right_result") print_pose(cvh.Rodrigues(P_right_result[0:3, 0:3]), P_right_result[0:3, 3:4]) print() else: print("<skipped: no non-planar objects have been selected>") print() print("Points:") print("Chessboard") print("coords = \\\n", map(list, objp_result[:num_planar, :])) print() if has_nonplanar: print("Non-planar geometry") print("coords_nonplanar = \\\n", map(list, objp_result[num_planar:, :])) print() ### Return to remember last manually matched successful non-planar imagepoints return nonplanar_left, nonplanar_right
def realtime_pose_estimation(device_id, filename_base_extrinsics, cameraMatrix, distCoeffs, objp, boardSize): """ This interactive demo will track a chessboard in realtime using a webcam, and the WORLD axis-system will be drawn on it: [X Y Z] = [red green blue] Further on you will see some data in the bottom-right corner, this indicates both the pose of the current image w.r.t. the WORLD axis-system, as well as the pose of the current image w.r.t. the previous keyframe pose. To create a new keyframe while running, press SPACE. Each time a new keyframe is generated, the corresponding image and data (in txt-format) is written to the 'filename_base_extrinsics' folder. All poses are defined in the WORLD axis-system, the rotation notation follows axis-angle representation: '<unit vector> * <magnitude (degrees)>'. To quit, press ESC. """ cv2.namedWindow("Image (with axis-system)") fontFace = cv2.FONT_HERSHEY_DUPLEX fontScale = 0.5 mlt = cvh.MultilineText() cap = cv2.VideoCapture(device_id) imageNr = 0 # keyframe image id rvec_prev = np.zeros((3, 1)) rvec = None tvec_prev = np.zeros((3, 1)) tvec = None # Loop until 'q' or ESC pressed last_key_pressed = 0 while not last_key_pressed in (ord('q'), 27): ret_, img = cap.read() ret, corners = cvh.extractChessboardFeatures(img, boardSize) # If valid features found, solve for 'rvec' and 'tvec' if ret == True: ret, rvec, tvec = cv2.solvePnP( # TODO: use Ransac version for other types of features objp, corners, cameraMatrix, distCoeffs) # Draw axis-system cvh.drawAxisSystem(img, cameraMatrix, distCoeffs, rvec, tvec) # OpenCV's 'rvec' and 'tvec' seem to be defined as follows: # 'rvec': rotation transformation: transforms points from "WORLD axis-system -> CAMERA axis-system" # 'tvec': translation of "CAMERA center -> WORLD center", all defined in the "CAMERA axis-system" rvec *= -1 # convert to: "CAMERA axis-system -> WORLD axis-system", equivalent to rotation of CAMERA axis-system w.r.t. WORLD axis-system tvec = cvh.Rodrigues(rvec).dot( tvec) # bring to "WORLD axis-system", ... tvec *= -1 # ... and change direction to "WORLD center -> CAMERA center" # Calculate pose relative to last keyframe rvec_rel = trfm.delta_rvec(rvec, rvec_prev) tvec_rel = tvec - tvec_prev # Extract axis and angle, to enhance representation rvec_axis, rvec_angle = trfm.axis_and_angle_from_rvec(rvec) rvec_rel_axis, rvec_rel_angle = trfm.axis_and_angle_from_rvec( rvec_rel) # Draw pose information texts = [] texts.append("Current pose:") texts.append(" Rvec: %s * %.1fdeg" % (format3DVector(rvec_axis), degrees(rvec_angle))) texts.append(" Tvec: %s" % format3DVector(tvec)) texts.append("Relative to previous pose:") texts.append( " Rvec: %s * %.1fdeg" % (format3DVector(rvec_rel_axis), degrees(rvec_rel_angle))) texts.append(" Tvec: %s" % format3DVector(tvec_rel)) mlt.text(texts[0], fontFace, fontScale * 1.5, rgb(150, 0, 0), thickness=2) mlt.text(texts[1], fontFace, fontScale, rgb(255, 0, 0)) mlt.text(texts[2], fontFace, fontScale, rgb(255, 0, 0)) mlt.text(texts[3], fontFace, fontScale * 1.5, rgb(150, 0, 0), thickness=2) mlt.text(texts[4], fontFace, fontScale, rgb(255, 0, 0)) mlt.text(texts[5], fontFace, fontScale, rgb(255, 0, 0)) mlt.putText(img, (img.shape[1], img.shape[0])) # put text in bottom-right corner # Show Image cv2.imshow("Image (with axis-system)", img) mlt.clear() # Save keyframe image when SPACE is pressed last_key_pressed = cv2.waitKey(1) & 0xFF if last_key_pressed == ord(' ') and ret: filename = filename_base_extrinsics + str(imageNr) cv2.imwrite(filename + ".jpg", img) # write image to jpg-file textTotal = '\n'.join(texts) open(filename + ".txt", 'w').write(textTotal) # write data to txt-file print("Saved keyframe image+data to", filename, ":") print(textTotal) imageNr += 1 rvec_prev = rvec tvec_prev = tvec
def main(): global boardSize global cameraMatrix, distCoeffs, imageSize global max_OF_error, max_lost_tracks_ratio global keypoint_coverage_radius #, min_keypoint_coverage global target_amount_keypoints, corner_quality_level, corner_min_dist global homography_condition_threshold global max_solvePnP_reproj_error, max_2nd_solvePnP_reproj_error, max_fundMat_reproj_error global max_solvePnP_outlier_ratio, max_2nd_solvePnP_outlier_ratio, solvePnP_use_extrinsic_guess # Initially known data boardSize = (8, 6) color_palette, color_palette_size = prepare_color_palette( 2, 3, 4) # used to identify 3D point group ids cameraMatrix, distCoeffs, imageSize = \ load_camera_intrinsics(os.path.join("..", "..", "datasets", "webcam", "camera_intrinsics.txt")) #cameraMatrix, distCoeffs, imageSize = \ #load_camera_intrinsics(os.path.join("..", "..", "..", "ARDrone2_tests", "camera_calibration", "live_video", "camera_intrinsics_front.txt")) # Select working (or 'testing') set from glob import glob images = sorted( glob( os.path.join("..", "..", "datasets", "webcam", "captures2", "*.jpeg"))) #images = sorted(glob(os.path.join("..", "..", "..", "ARDrone2_tests", "flying_front", "lowres", "drone0", "*.jpg"))) # Setup some visualization helpers composite2D_painter = Composite2DPainter("composite 2D", imageSize) composite3D_painter = Composite3DPainter( "composite 3D", trfm.P_from_R_and_t(cvh.Rodrigues((pi, 0., 0.)), np.array([[0., 0., 40.]]).T), (1280, 720)) ### Tweaking parameters ### # OF calculation max_OF_error = 12. max_lost_tracks_ratio = 0.5 # keypoint_coverage keypoint_coverage_radius = int(max_OF_error) #min_keypoint_coverage = 0.2 # goodFeaturesToTrack target_amount_keypoints = int( round( (imageSize[0] * imageSize[1]) / (pi * keypoint_coverage_radius**2))) # target is entire image full print "target_amount_keypoints:", target_amount_keypoints corner_quality_level = 0.01 corner_min_dist = keypoint_coverage_radius # keyframe_test homography_condition_threshold = 500 # defined as ratio between max and min singular values # reprojection error max_solvePnP_reproj_error = 4. #0.5 # TODO: revert to a lower number max_2nd_solvePnP_reproj_error = max_solvePnP_reproj_error / 2 # be more strict in a 2nd iteration, used after 1st pass of triangulation max_fundMat_reproj_error = 2.0 # solvePnP max_solvePnP_outlier_ratio = 0.3 max_2nd_solvePnP_outlier_ratio = 1. # used in 2nd iteration, after 1st pass of triangulation solvePnP_use_extrinsic_guess = False # TODO: set to True and see whether the 3D results are better # Init imgs = [] imgs_gray = [] objp = [] # 3D points objp_groups = [ ] # 3D point group ids, each new batch of detected points is put in a separate group group_id = 0 # current 3D point group id rvecs, rvecs_keyfr = [], [] tvecs, tvecs_keyfr = [], [] # Start frame requires special treatment # Start frame : read image and detect 2D points imgs.append(cv2.imread(images[0])) base_img = imgs[0] imgs_gray.append(cv2.cvtColor(imgs[0], cv2.COLOR_BGR2GRAY)) ret, new_imgp = cvh.extractChessboardFeatures(imgs[0], boardSize) if not ret: print "First image must contain the entire chessboard!" return # Start frame : define a priori 3D points objp = prepare_object_points(boardSize) objp_groups = np.zeros((objp.shape[0]), dtype=np.int) group_id += 1 # Start frame : setup linking data-structures base_imgp = new_imgp # 2D points all_idxs_tmp = np.arange(len(base_imgp)) triangl_idxs = set(all_idxs_tmp) nontriangl_idxs = set() imgp_to_objp_idxs = np.array(sorted(triangl_idxs), dtype=int) # Start frame : get absolute pose ret, rvec, tvec = cv2.solvePnP( # assume first frame is a proper frame with chessboard fully in-sight objp, new_imgp, cameraMatrix, distCoeffs) print "solvePnP reproj_error init:", reprojection_error( objp, new_imgp, rvec, tvec, cameraMatrix, distCoeffs)[0] rvecs.append(rvec) tvecs.append(tvec) rvec_keyfr = rvec tvec_keyfr = tvec rvecs_keyfr.append(rvec_keyfr) tvecs_keyfr.append(tvec_keyfr) # Start frame : add other points mask_img = keypoint_mask(new_imgp) to_add = target_amount_keypoints - len(new_imgp) imgp_extra = cv2.goodFeaturesToTrack(imgs_gray[0], to_add, corner_quality_level, corner_min_dist, None, mask_img).reshape((-1, 2)) cv2.imshow( "img", cv2.drawKeypoints(imgs[0], [cv2.KeyPoint(p[0], p[1], 7.) for p in imgp_extra], color=rgb(0, 0, 255))) cv2.waitKey() print "added:", len(imgp_extra) base_imgp, new_imgp, imgp_to_objp_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp = idxs_rebase_and_add_imgp( imgp_extra, base_imgp, new_imgp, imgp_to_objp_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp) ret = 2 # indicate keyframe # Draw 3D points info of current frame print "Drawing composite 2D image" composite2D_painter.draw(imgs[0], rvec, tvec, ret, 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) # Draw 3D points info of all frames print "Drawing composite 3D image (keys: LEFT/RIGHT/UP/DOWN/PAGEUP/PAGEDOWN/HOME/END)" composite3D_painter.draw(rvec, tvec, ret, triangl_idxs, imgp_to_objp_idxs, objp, objp_groups, color_palette, color_palette_size) for i in range(1, len(images)): # Frame[i-1] -> Frame[i] print "\nFrame[%s] -> Frame[%s]" % (i - 1, i) print " processing '", images[i], "':" cur_img = cv2.imread(images[i]) imgs.append(cur_img) imgs_gray.append(cv2.cvtColor(imgs[-1], cv2.COLOR_BGR2GRAY)) ret, base_imgp, new_imgp, triangl_idxs, nontriangl_idxs, imgp_to_objp_idxs, all_idxs_tmp, objp, objp_groups, group_id, rvec, tvec, rvec_keyfr, tvec_keyfr, base_img = \ handle_new_frame(base_imgp, new_imgp, imgs[-2], imgs_gray[-2], imgs[-1], imgs_gray[-1], triangl_idxs, nontriangl_idxs, imgp_to_objp_idxs, all_idxs_tmp, objp, objp_groups, group_id, rvec_keyfr, tvec_keyfr, base_img) if ret: rvecs.append(rvec) tvecs.append(tvec) if ret == 2: # frame is a keyframe rvecs_keyfr.append(rvec_keyfr) tvecs_keyfr.append(tvec_keyfr) else: # frame rejected del imgs[-1] del imgs_gray[-1] # Draw 3D points info of current frame print "Drawing composite 2D image" composite2D_painter.draw(cur_img, rvec, tvec, ret, 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) # Draw 3D points info of all frames print "Drawing composite 3D image (keys: LEFT/RIGHT/UP/DOWN/PAGEUP/PAGEDOWN)" composite3D_painter.draw(rvec, tvec, ret, triangl_idxs, imgp_to_objp_idxs, objp, objp_groups, color_palette, color_palette_size)
def handle_new_frame( base_imgp, # includes 2D points of both triangulated as not-yet triangl points of last keyframe prev_imgp, # includes 2D points of last frame prev_img, prev_img_gray, new_img, new_img_gray, triangl_idxs, nontriangl_idxs, # indices of 2D points in base_imgp imgp_to_objp_idxs, # indices from 2D points in base_imgp to 3D points in objp all_idxs_tmp, # list of idxs of 2D points in base_imgp, matches prev_imgp to base_imgp objp, # triangulated 3D points objp_groups, group_id, # corresponding group ids of triangulated 3D points, and current group id rvec_keyfr, tvec_keyfr, # rvec and tvec of last keyframe base_img): # used for debug # Save initial indexing state triangl_idxs_old = set(triangl_idxs) nontriangl_idxs_old = set(nontriangl_idxs) all_idxs_tmp_old = np.array(all_idxs_tmp) # Calculate OF (Optical Flow), and filter outliers based on OF error new_imgp, status_OF, err_OF = cv2.calcOpticalFlowPyrLK( prev_img_gray, new_img_gray, prev_imgp ) # WARNING: OpenCV can output corrupted values in 'status_OF': "RuntimeWarning: invalid value encountered in less" new_to_prev_idxs = np.where( np.logical_and((status_OF.reshape(-1) == 1), (err_OF.reshape(-1) < max_OF_error)))[0] # If there is too much OF error in the entire image, simply reject the frame lost_tracks_ratio = (len(prev_imgp) - len(new_to_prev_idxs)) / float( len(prev_imgp)) print "# points lost because of excessive OF error / # points before: ", len( prev_imgp) - len(new_to_prev_idxs), "/", len( prev_imgp), "=", lost_tracks_ratio if lost_tracks_ratio > max_lost_tracks_ratio: # reject frame print "REJECTED: I lost track of all points!\n" return False, base_imgp, prev_imgp, triangl_idxs_old, nontriangl_idxs_old, imgp_to_objp_idxs, all_idxs_tmp_old, objp, objp_groups, group_id, None, None, rvec_keyfr, tvec_keyfr, base_img # Save matches by idxs preserve_idxs = set(all_idxs_tmp[new_to_prev_idxs]) triangl_idxs, nontriangl_idxs, all_idxs_tmp = idxs_update_by_idxs( preserve_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp) if len(triangl_idxs) < 8: # solvePnP uses 8-point algorithm print "REJECTED: I lost track of too many already-triangulated points, so we can't do solvePnP() anymore...\n" return False, base_imgp, prev_imgp, triangl_idxs_old, nontriangl_idxs_old, imgp_to_objp_idxs, all_idxs_tmp_old, objp, objp_groups, group_id, None, None, rvec_keyfr, tvec_keyfr, base_img new_imgp = new_imgp[new_to_prev_idxs] #cv2.cornerSubPix( # TODO: activate this secret weapon <-- hmm, actually seems to make it worse #new_img_gray, new_imgp, #(corner_min_dist,corner_min_dist), # window #(-1,-1), # deadzone #(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) ) # termination criteria ##corners = corners.reshape(-1, 2) # Do solvePnPRansac() on current frame's (new_imgp) already triangulated points, ... triangl_idxs_array = np.array( sorted(triangl_idxs)) # select already-triangulated point-indices filtered_triangl_imgp = idxs_get_new_imgp_by_idxs( triangl_idxs, new_imgp, all_idxs_tmp) # collect corresponding image-points filtered_triangl_objp = objp[imgp_to_objp_idxs[ triangl_idxs_array]] # collect corresponding object-points print "Doing solvePnP() on", filtered_triangl_objp.shape[0], "points" rvec_, tvec_, inliers = cv2.solvePnPRansac( # perform solvePnPRansac() to identify outliers, force to obey max_solvePnP_outlier_ratio filtered_triangl_objp, filtered_triangl_imgp, cameraMatrix, distCoeffs, minInliersCount=int( ceil((1 - max_solvePnP_outlier_ratio) * len(triangl_idxs))), reprojectionError=max_solvePnP_reproj_error) # ... if ratio of 'inliers' vs input is too low, reject frame, ... if inliers == None: # inliers is empty => reject frame print "REJECTED: No inliers based on solvePnP()!\n" return False, base_imgp, prev_imgp, triangl_idxs_old, nontriangl_idxs_old, imgp_to_objp_idxs, all_idxs_tmp_old, objp, objp_groups, group_id, None, None, rvec_keyfr, tvec_keyfr, base_img inliers = inliers.reshape(-1) solvePnP_outlier_ratio = (len(triangl_idxs) - len(inliers)) / float( len(triangl_idxs)) print "solvePnP_outlier_ratio:", solvePnP_outlier_ratio if solvePnP_outlier_ratio > max_solvePnP_outlier_ratio or len( inliers) < 8: # reject frame if solvePnP_outlier_ratio > max_solvePnP_outlier_ratio: print "REJECTED: Not enough inliers (ratio) based on solvePnP()!\n" else: print "REJECTED: Not enough inliers (absolute) based on solvePnP() to perform (non-RANSAC) solvePnP()!\n" return False, base_imgp, prev_imgp, triangl_idxs_old, nontriangl_idxs_old, imgp_to_objp_idxs, all_idxs_tmp_old, objp, objp_groups, group_id, None, None, rvec_keyfr, tvec_keyfr, base_img # <DEBUG: visualize reprojection error> TODO: remove reproj_error, imgp_reproj1 = reprojection_error(filtered_triangl_objp, filtered_triangl_imgp, rvec_, tvec_, cameraMatrix, distCoeffs) print "solvePnP reproj_error:", reproj_error i3 = np.array(new_img) try: for imgppr, imgppp in zip(filtered_triangl_imgp, imgp_reproj1): cvh.line(i3, imgppr.T, imgppp.T, rgb(255, 0, 0)) except OverflowError: print "WARNING: OverflowError!" cv2.imshow("img", i3) cv2.waitKey() # </DEBUG> # ... then do solvePnP() on inliers, to get the current frame's pose estimation, ... triangl_idxs_array = triangl_idxs_array[ inliers] # select inliers among all already-triangulated point-indices filtered_triangl_imgp, filtered_triangl_objp = filtered_triangl_imgp[ inliers], filtered_triangl_objp[inliers] preserve_idxs = set(triangl_idxs_array) | nontriangl_idxs new_imgp = idxs_get_new_imgp_by_idxs(preserve_idxs, new_imgp, all_idxs_tmp) triangl_idxs, nontriangl_idxs, all_idxs_tmp = idxs_update_by_idxs( # update indices to only preserve inliers preserve_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp) ret, rvec, tvec = cv2.solvePnP( # perform solvePnP() to estimate the pose filtered_triangl_objp, filtered_triangl_imgp, cameraMatrix, distCoeffs) # .. finally do a check on the average reprojection error, and reject frame if too high. reproj_error, imgp_reproj = reprojection_error(filtered_triangl_objp, filtered_triangl_imgp, rvec, tvec, cameraMatrix, distCoeffs) print "solvePnP refined reproj_error:", reproj_error if reproj_error > max_solvePnP_reproj_error: # reject frame print "REJECTED: Too high reprojection error based on pose estimate of solvePnP()!\n" return False, base_imgp, prev_imgp, triangl_idxs_old, nontriangl_idxs_old, imgp_to_objp_idxs, all_idxs_tmp_old, objp, objp_groups, group_id, None, None, rvec_keyfr, tvec_keyfr, base_img # <DEBUG: verify poses by reprojection error> TODO: remove i0 = draw_axis_system(np.array(new_img), rvec, tvec, cameraMatrix, distCoeffs) try: for imgppp in imgp_reproj1: cvh.circle(i0, imgppp.T, 2, rgb(255, 0, 0), thickness=-1) except OverflowError: print "WARNING: OverflowError!" for imgppp in imgp_reproj: cvh.circle(i0, imgppp.T, 2, rgb(0, 255, 255), thickness=-1) print "cur img check" cv2.imshow("img", i0) cv2.waitKey() # </DEBUG> # <DEBUG: verify OpticalFlow motion on preserved inliers> TODO: remove cv2.imshow( "img", cvh.drawKeypointsAndMotion(new_img, base_imgp[all_idxs_tmp], new_imgp, rgb(0, 0, 255))) cv2.waitKey() # </DEBUG> # Check whether we got a new keyframe is_keyframe = keyframe_test(base_imgp[all_idxs_tmp], new_imgp) print "is_keyframe:", is_keyframe if is_keyframe: # If some points are not yet triangulated, do it now: if nontriangl_idxs: # First do triangulation of not-yet triangulated points using initial pose estimation, ... nontriangl_idxs_array = np.array(sorted( nontriangl_idxs)) # select not-yet-triangulated point-indices imgp0 = base_imgp[ nontriangl_idxs_array] # collect corresponding image-points of last keyframe imgp1 = idxs_get_new_imgp_by_idxs( nontriangl_idxs, new_imgp, all_idxs_tmp ) # collect corresponding image-points of current frame # <DEBUG: check sanity of input to triangulation function> TODO: remove check_triangulation_input(base_img, new_img, imgp0, imgp1, rvec_keyfr, tvec_keyfr, rvec, tvec, cameraMatrix, distCoeffs) # </DEBUG> imgpnrm0 = cv2.undistortPoints(np.array( [imgp0]), cameraMatrix, distCoeffs)[ 0] # undistort and normalize to homogenous coordinates imgpnrm1 = cv2.undistortPoints(np.array([imgp1]), cameraMatrix, distCoeffs)[0] objp_done = linear_LS_triangulation( # triangulate imgpnrm0.T, trfm.P_from_R_and_t(cvh.Rodrigues(rvec_keyfr), tvec_keyfr), # data from last keyframe imgpnrm1.T, trfm.P_from_R_and_t(cvh.Rodrigues(rvec), tvec)) # data from current frame objp_done = objp_done.T # <DEBUG: check reprojection error of the new freshly triangulated points, based on both pose estimates of keyframe and current cam> TODO: remove print "triangl_reproj_error 0:", reprojection_error( objp_done, imgp0, rvec_keyfr, tvec_keyfr, cameraMatrix, distCoeffs)[0] print "triangl_reproj_error 1:", reprojection_error( objp_done, imgp1, rvec, tvec, cameraMatrix, distCoeffs)[0] # </DEBUG> filtered_triangl_objp = np.concatenate( (filtered_triangl_objp, objp_done)) # collect all desired object-points filtered_triangl_imgp = np.concatenate( (filtered_triangl_imgp, imgp1)) # collect corresponding image-points of current frame # ... then do solvePnP() on all preserved points ('inliers') to refine pose estimation, ... ret, rvec, tvec = cv2.solvePnP( # perform solvePnP(), we start from the initial pose estimation filtered_triangl_objp, filtered_triangl_imgp, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess=True) print "total triangl_reproj_error 1 refined:", reprojection_error( filtered_triangl_objp, filtered_triangl_imgp, rvec, tvec, cameraMatrix, distCoeffs)[0] # TODO: remove # ... then do re-triangulation of 'inliers_objp_done' using refined pose estimation. objp_done = linear_LS_triangulation( # triangulate imgpnrm0.T, trfm.P_from_R_and_t(cvh.Rodrigues(rvec_keyfr), tvec_keyfr), # data from last keyframe imgpnrm1.T, trfm.P_from_R_and_t(cvh.Rodrigues(rvec), tvec)) # data from current frame objp_done = objp_done.T # <DEBUG: check reprojection error of the new freshly (refined) triangulated points, based on both pose estimates of keyframe and current cam> TODO: remove print "triangl_reproj_error 0 refined:", reprojection_error( objp_done, imgp0, rvec_keyfr, tvec_keyfr, cameraMatrix, distCoeffs)[0] print "triangl_reproj_error 1 refined:", reprojection_error( objp_done, imgp1, rvec, tvec, cameraMatrix, distCoeffs)[0] # </DEBUG> # Update image-points and indices, and store the newly triangulated object-points and assign them the current group id new_imgp = filtered_triangl_imgp triangl_idxs, nontriangl_idxs, all_idxs_tmp = idxs_update_by_idxs( preserve_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp) objp_groups_done = np.empty((len(objp_done)), dtype=int) objp_groups_done.fill(group_id) # assign to current 'group_id' ( objp, objp_groups ), imgp_to_objp_idxs, triangl_idxs, nontriangl_idxs = idxs_add_objp( (objp_done, objp_groups_done), preserve_idxs - triangl_idxs, (objp, objp_groups), imgp_to_objp_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp) ## <DEBUG: check intermediate outlier filtering> TODO: remove #i4 = np.array(new_img) #objp_test = objp[imgp_to_objp_idxs[np.array(sorted(triangl_idxs))]] #imgp_test = idxs_get_new_imgp_by_idxs(triangl_idxs, new_imgp, all_idxs_tmp) ##print imgp_test - filtered_triangl_imgp #reproj_error, imgp_reproj4 = reprojection_error(objp_test, imgp_test, rvec, tvec, cameraMatrix, distCoeffs) #print "checking both 2", reproj_error #for imgppr, imgppp in zip(imgp_test, imgp_reproj4): cvh.line(i4, imgppr.T, imgppp.T, rgb(255,0,0)) #cv2.imshow("img", i4) #cv2.waitKey() ## </DEBUG> # Check whether we should add new image-points mask_img = keypoint_mask( new_imgp ) # generate mask that covers all image-points (with a certain radius) print "coverage:", 1 - cv2.countNonZero(mask_img) / float( mask_img.size) # TODO: remove: unused to_add = target_amount_keypoints - len( new_imgp) # limit the amount of to-be-added image-points # Add new image-points if to_add > 0: print "to_add:", to_add imgp_extra = cv2.goodFeaturesToTrack(new_img_gray, to_add, corner_quality_level, corner_min_dist, None, mask_img).reshape((-1, 2)) print "added:", len(imgp_extra) group_id += 1 # create a new group to assign the new batch of points to, later on else: imgp_extra = zeros((0, 2)) print "adding zero new points" base_imgp, new_imgp, imgp_to_objp_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp = idxs_rebase_and_add_imgp( # update indices to include new image-points imgp_extra, base_imgp, new_imgp, imgp_to_objp_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp) # <DEBUG: visualize newly added points> TODO: remove cv2.imshow( "img", cv2.drawKeypoints( new_img, [cv2.KeyPoint(p[0], p[1], 7.) for p in imgp_extra], color=rgb(0, 0, 255))) cv2.waitKey() # </DEBUG> # Now this frame becomes the base (= keyframe) rvec_keyfr = rvec tvec_keyfr = tvec base_img = new_img # Successfully return return True + int( is_keyframe ), base_imgp, new_imgp, triangl_idxs, nontriangl_idxs, imgp_to_objp_idxs, all_idxs_tmp, objp, objp_groups, group_id, rvec, tvec, rvec_keyfr, tvec_keyfr, base_img
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