def track_reference_frame(self, f_ref, f_cur, name=''): print('>>>> tracking reference %d ...' % (f_ref.id)) if f_ref is None: return # find keypoint matches between f_cur and kf_ref print('matching keypoints with ', Frame.feature_matcher.type.name) self.timer_match.start() idxs_cur, idxs_ref = match_frames(f_cur, f_ref) self.timer_match.refresh() self.num_matched_kps = idxs_cur.shape[0] print("# keypoints matched: %d " % self.num_matched_kps) if kUseEssentialMatrixFitting: # estimate camera orientation and inlier matches by fitting and essential matrix (see the limitations above) idxs_ref, idxs_cur = self.estimate_pose_by_fitting_ess_mat( f_ref, f_cur, idxs_ref, idxs_cur) if kUseDynamicDesDistanceTh: self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat( f_ref, f_cur, idxs_ref, idxs_cur) # propagate map point matches from kf_ref to f_cur (do not override idxs_ref, idxs_cur) num_found_map_pts_inter_frame, idx_ref_prop, idx_cur_prop = propagate_map_point_matches( f_ref, f_cur, idxs_ref, idxs_cur, max_descriptor_distance=self.descriptor_distance_sigma) print("# matched map points in prev frame: %d " % num_found_map_pts_inter_frame) if kDebugDrawMatches and True: img_matches = draw_feature_matches(f_ref.img, f_cur.img, f_ref.kps[idx_ref_prop], f_cur.kps[idx_cur_prop], f_ref.sizes[idx_ref_prop], f_cur.sizes[idx_cur_prop], horizontal=False) cv2.imshow('tracking frame (no projection) - matches', img_matches) cv2.waitKey(1) # store tracking info (for possible reuse) self.idxs_ref = idxs_ref self.idxs_cur = idxs_cur # f_cur pose optimization using last matches with kf_ref: # here, we use first guess of f_cur pose and propated map point matches from f_ref (matched keypoints) self.pose_optimization(f_cur, name) # update matched map points; discard outliers detected in last pose optimization num_matched_points = f_cur.clean_outlier_map_points() print(' # num_matched_map_points: %d' % (self.num_matched_map_points)) #print(' # matched points: %d' % (num_matched_points) ) if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackFrame: f_cur.remove_frame_views(idxs_cur) f_cur.reset_points() Printer.red( 'failure in tracking reference %d, # matched map points: %d' % (f_ref.id, self.num_matched_map_points)) self.pose_is_ok = False
# Drawing #============================================ img_matched_inliers = None if mask is not None: # Build arrays of matched inliers mask_idxs = (mask.ravel() == 1) kps1_matched_inliers = kps1_matched[mask_idxs] kps1_size_inliers = kps1_size[mask_idxs] des1_matched_inliers = des1_matched[mask_idxs][:] kps2_matched_inliers = kps2_matched[mask_idxs] kps2_size_inliers = kps2_size[mask_idxs] des2_matched_inliers = des2_matched[mask_idxs][:] print('num inliers: ', len(kps1_matched_inliers)) print('inliers percentage: ', len(kps1_matched_inliers)/max(len(kps1_matched),1.)*100,'%') sigma_mad_inliers, dists = descriptor_sigma_mad(des1_matched_inliers,des2_matched_inliers,descriptor_distances=feature_tracker.descriptor_distances) print('3 x sigma-MAD of descriptor distances (inliers): ', 3 * sigma_mad) #print('distances: ', dists) img_matched_inliers = draw_feature_matches(img1, img2, kps1_matched_inliers, kps2_matched_inliers, kps1_size_inliers, kps2_size_inliers,draw_horizontal_layout) img_matched = draw_feature_matches(img1, img2, kps1_matched, kps2_matched, kps1_size, kps2_size,draw_horizontal_layout) fig1 = MPlotFigure(img_matched, title='All matches') if img_matched_inliers is not None: fig2 = MPlotFigure(img_matched_inliers, title='Inlier matches') MPlotFigure.show()
def track_previous_frame(self, f_ref, f_cur): print('>>>> tracking previous frame ...') is_search_frame_by_projection_failure = False use_search_frame_by_projection = self.motion_model.is_ok and kUseSearchFrameByProjection and kUseMotionModel if use_search_frame_by_projection: # search frame by projection: match map points observed in f_ref with keypoints of f_cur print('search frame by projection') search_radius = Parameters.kMaxReprojectionDistanceFrame f_cur.reset_points() self.timer_seach_frame_proj.start() idxs_ref, idxs_cur, num_found_map_pts = search_frame_by_projection( f_ref, f_cur, max_reproj_distance=search_radius, max_descriptor_distance=self.descriptor_distance_sigma) self.timer_seach_frame_proj.refresh() self.num_matched_kps = len(idxs_cur) print("# matched map points in prev frame: %d " % self.num_matched_kps) # if not enough map point matches consider a larger search radius if self.num_matched_kps < Parameters.kMinNumMatchedFeaturesSearchFrameByProjection: f_cur.remove_frame_views(idxs_cur) f_cur.reset_points() idxs_ref, idxs_cur, num_found_map_pts = search_frame_by_projection( f_ref, f_cur, max_reproj_distance=2 * search_radius, max_descriptor_distance=0.5 * self.descriptor_distance_sigma) self.num_matched_kps = len(idxs_cur) Printer.orange( "# matched map points in prev frame (wider search): %d " % self.num_matched_kps) if kDebugDrawMatches and True: img_matches = draw_feature_matches(f_ref.img, f_cur.img, f_ref.kps[idxs_ref], f_cur.kps[idxs_cur], f_ref.sizes[idxs_ref], f_cur.sizes[idxs_cur], horizontal=False) cv2.imshow('tracking frame by projection - matches', img_matches) cv2.waitKey(1) if self.num_matched_kps < Parameters.kMinNumMatchedFeaturesSearchFrameByProjection: f_cur.remove_frame_views(idxs_cur) f_cur.reset_points() is_search_frame_by_projection_failure = True Printer.red( 'Not enough matches in search frame by projection: ', self.num_matched_kps) else: # search frame by projection was successful if kUseDynamicDesDistanceTh: self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat( f_ref, f_cur, idxs_ref, idxs_cur) # store tracking info (for possible reuse) self.idxs_ref = idxs_ref self.idxs_cur = idxs_cur # f_cur pose optimization 1: # here, we use f_cur pose as first guess and exploit the matched map point of f_ref self.pose_optimization(f_cur, 'proj-frame-frame') # update matched map points; discard outliers detected in last pose optimization num_matched_points = f_cur.clean_outlier_map_points() print(' # num_matched_map_points: %d' % (self.num_matched_map_points)) #print(' # matched points: %d' % (num_matched_points) ) if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackFrame: Printer.red( 'failure in tracking previous frame, # matched map points: ', self.num_matched_map_points) self.pose_is_ok = False if not use_search_frame_by_projection or is_search_frame_by_projection_failure: self.track_reference_frame(f_ref, f_cur, 'match-frame-frame')
print('search for triangulation...') timer.start() idxs_ref, idxs_cur, num_found_matches, img_cur_epi = search_frame_for_triangulation_test( f_ref, f_cur, img_cur, img1=img_ref) # test #idxs_ref, idxs_cur, num_found_matches, img_cur_epi = search_frame_for_triangulation(f_ref, f_cur) elapsed = timer.elapsed() print('time:', elapsed) print('# found matches:', num_found_matches) N = len(idxs_ref) pts_ref = f_ref.kpsu[idxs_ref[:N]] pts_cur = f_cur.kpsu[idxs_cur[:N]] img_ref, img_cur = draw_points2(img_ref, img_cur, pts_ref, pts_cur) img_matches = draw_feature_matches(img_ref, img_cur, pts_ref, pts_cur, horizontal=False) if img_cur_epi is not None: fig1 = MPlotFigure(img_cur_epi, title='points and epipolar lines') fig_ref = MPlotFigure(img_ref, title='image ref') fig_cur = MPlotFigure(img_cur, title='image cur') fig_matches = MPlotFigure(img_matches, title='image matches') MPlotFigure.show()