def local_BA(self): # local optimization self.time_local_opt.start() err = self.map.locally_optimize(kf_ref=self.kf_cur, abort_flag=self.opt_abort_flag) self.time_local_opt.refresh() print("local optimization error^2: %f" % err) num_kf_ref_tracked_points = self.kf_cur.num_tracked_points(kNumMinObsForKeyFrameDefault) # number of tracked points in k_ref Printer.purple('KF(%d) #points: %d ' %(self.kf_cur.id, num_kf_ref_tracked_points))
def need_new_keyframe(self, f_cur): num_keyframes = self.map.num_keyframes() nMinObs = kNumMinObsForKeyFrameDefault if num_keyframes <= 2: nMinObs = 2 # if just two keyframes then we can have just two observations num_kf_ref_tracked_points = self.kf_ref.num_tracked_points( nMinObs) # number of tracked points in k_ref num_f_cur_tracked_points = f_cur.num_matched_inlier_map_points( ) # number of inliers in f_cur Printer.purple('F(%d) #points: %d, KF(%d) #points: %d ' % (f_cur.id, num_f_cur_tracked_points, self.kf_ref.id, num_kf_ref_tracked_points)) if kLogKFinfoToFile: self.kf_info_logger.info( 'F(%d) #points: %d, KF(%d) #points: %d ' % (f_cur.id, num_f_cur_tracked_points, self.kf_ref.id, num_kf_ref_tracked_points)) self.num_kf_ref_tracked_points = num_kf_ref_tracked_points is_local_mapping_idle = self.local_mapping.is_idle() local_mapping_queue_size = self.local_mapping.queue_size() print('is_local_mapping_idle: ', is_local_mapping_idle, ', local_mapping_queue_size: ', local_mapping_queue_size) # condition 1: more than "max_frames_between_kfs" have passed from last keyframe insertion cond1 = f_cur.id >= (self.kf_last.id + self.max_frames_between_kfs) # condition 2: more than "min_frames_between_kfs" have passed and local mapping is idle cond2 = (f_cur.id >= (self.kf_last.id + self.min_frames_between_kfs)) & is_local_mapping_idle #cond2 = (f_cur.id >= (self.kf_last.id + self.min_frames_between_kfs)) # condition 3: few tracked features compared to reference keyframe cond3 = (num_f_cur_tracked_points < num_kf_ref_tracked_points * Parameters.kThNewKfRefRatio) and ( num_f_cur_tracked_points > Parameters.kNumMinPointsForNewKf) #print('KF conditions: %d %d %d' % (cond1, cond2, cond3) ) ret = (cond1 or cond2) and cond3 if ret: if is_local_mapping_idle: return True else: self.local_mapping.interrupt_optimization() if True: if local_mapping_queue_size <= 3: return True else: return False else: return False else: return False