def check_triangulation_input(base_img, new_img, imgp0, imgp1, rvec_keyfr, tvec_keyfr, rvec, tvec, cameraMatrix, distCoeffs): img = cvh.drawKeypointsAndMotion(base_img, imgp1, imgp0, rgb(0, 0, 255)) draw_axis_system(img, rvec_keyfr, tvec_keyfr, cameraMatrix, distCoeffs) print "base_img check" cv2.imshow("img", img) cv2.waitKey() img = cvh.drawKeypointsAndMotion(new_img, imgp0, imgp1, rgb(0, 0, 255)) draw_axis_system(img, rvec, tvec, cameraMatrix, distCoeffs) print "new_img check" cv2.imshow("img", img) cv2.waitKey()
def check_triangulation_input(base_img, new_img, imgp0, imgp1, rvec_keyfr, tvec_keyfr, rvec, tvec, cameraMatrix, distCoeffs): img = cvh.drawKeypointsAndMotion(base_img, imgp1, imgp0, rgb(0,0,255)) draw_axis_system(img, rvec_keyfr, tvec_keyfr, cameraMatrix, distCoeffs) print "base_img check" cv2.imshow("img", img) cv2.waitKey() img = cvh.drawKeypointsAndMotion(new_img, imgp0, imgp1, rgb(0,0,255)) draw_axis_system(img, rvec, tvec, cameraMatrix, distCoeffs) print "new_img check" cv2.imshow("img", img) cv2.waitKey()
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 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