def create_new_map_points(self): print('>>>> creating new map points') total_new_pts = 0 local_keyframes = self.map.local_map.get_best_neighbors(self.kf_cur) print('local map keyframes: ', [kf.id for kf in local_keyframes if not kf.is_bad], ' + ', self.kf_cur.id, '...') match_idxs = defaultdict( lambda: (None, None) ) # dictionary of matches (kf_i, kf_j) -> (idxs_i,idxs_j) # precompute keypoint matches match_idxs = self.precompute_kps_matches(match_idxs, local_keyframes) for i, kf in enumerate(local_keyframes): if kf is self.kf_cur or kf.is_bad: continue if i > 0 and not self.queue.empty(): print('creating new map points *** interruption ***') return total_new_pts #print("adding map points for KFs (%d, %d)" % (self.kf_cur.id, kf.id)) # extract matches from precomputed map idxs_kf_cur, idxs_kf = match_idxs[(self.kf_cur, kf)] # find keypoint matches between self.kf_cur and kf # N.B.: all the matched keypoints computed by search_frame_for_triangulation() are without a corresponding map point idxs_cur, idxs, num_found_matches, _ = search_frame_for_triangulation( self.kf_cur, kf, idxs_kf_cur, idxs_kf, max_descriptor_distance=0.5 * self.descriptor_distance_sigma) if len(idxs_cur) > 0: # try to triangulate the matched keypoints that do not have a corresponding map point pts3d, mask_pts3d = triangulate_normalized_points( self.kf_cur.pose, kf.pose, self.kf_cur.kpsn[idxs_cur], kf.kpsn[idxs]) new_pts_count, _, list_added_points = self.map.add_points( pts3d, mask_pts3d, self.kf_cur, kf, idxs_cur, idxs, self.kf_cur.img, do_check=True) print("# added map points: %d for KFs (%d, %d)" % (new_pts_count, self.kf_cur.id, kf.id)) total_new_pts += new_pts_count self.recently_added_points.update(list_added_points) return total_new_pts
def initialize(self, f_cur, img_cur): if self.num_failures > kNumOfFailuresAfterWichNumMinTriangulatedPointsIsHalved: self.num_min_triangulated_points = 0.5 * Parameters.kInitializerNumMinTriangulatedPoints self.num_failures = 0 Printer.orange( 'Initializer: halved min num triangulated features to ', self.num_min_triangulated_points) # prepare the output out = InitializerOutput() is_ok = False #print('num frames: ', len(self.frames)) # if too many frames have passed, move the current idx_f_ref forward # this is just one very simple policy that can be used if self.f_ref is not None: if f_cur.id - self.f_ref.id > kMaxIdDistBetweenIntializingFrames: self.f_ref = self.frames[-1] # take last frame in the buffer #self.idx_f_ref = len(self.frames)-1 # take last frame in the buffer #self.idx_f_ref = self.frames.index(self.f_ref) # since we are using a deque, the code of the previous commented line is not valid anymore #print('*** idx_f_ref:',self.idx_f_ref) #self.f_ref = self.frames[self.idx_f_ref] f_ref = self.f_ref #print('ref fid: ',self.f_ref.id,', curr fid: ', f_cur.id, ', idxs_ref: ', self.idxs_ref) # append current frame self.frames.append(f_cur) # if the current frames do no have enough features exit if len(f_ref.kps) < self.num_min_features or len( f_cur.kps) < self.num_min_features: Printer.red('Inializer: ko - not enough features!') self.num_failures += 1 return out, is_ok # find keypoint matches idxs_cur, idxs_ref = match_frames(f_cur, f_ref, kFeatureMatchRatioTestInitializer) #print('|------------') #print('deque ids: ', [f.id for f in self.frames]) #print('initializing frames ', f_cur.id, ', ', f_ref.id) #print("# keypoint matches: ", len(idxs_cur)) Trc = self.estimatePose(f_ref.kpsn[idxs_ref], f_cur.kpsn[idxs_cur]) Tcr = inv_T(Trc) # Tcr w.r.t. ref frame f_ref.update_pose(np.eye(4)) f_cur.update_pose(Tcr) # remove outliers from keypoint matches by using the mask computed with inter frame pose estimation mask_idxs = (self.mask_match.ravel() == 1) self.num_inliers = sum(mask_idxs) #print('# keypoint inliers: ', self.num_inliers ) idx_cur_inliers = idxs_cur[mask_idxs] idx_ref_inliers = idxs_ref[mask_idxs] # create a temp map for initializing map = Map() f_ref.reset_points() f_cur.reset_points() #map.add_frame(f_ref) #map.add_frame(f_cur) kf_ref = KeyFrame(f_ref) kf_cur = KeyFrame(f_cur, img_cur) map.add_keyframe(kf_ref) map.add_keyframe(kf_cur) pts3d, mask_pts3d = triangulate_normalized_points( kf_cur.Tcw, kf_ref.Tcw, kf_cur.kpsn[idx_cur_inliers], kf_ref.kpsn[idx_ref_inliers]) new_pts_count, mask_points, _ = map.add_points( pts3d, mask_pts3d, kf_cur, kf_ref, idx_cur_inliers, idx_ref_inliers, img_cur, do_check=True, cos_max_parallax=Parameters.kCosMaxParallaxInitializer) #print("# triangulated points: ", new_pts_count) if new_pts_count > self.num_min_triangulated_points: err = map.optimize(verbose=False, rounds=20, use_robust_kernel=True) print("init optimization error^2: %f" % err) num_map_points = len(map.points) print("# map points: %d" % num_map_points) is_ok = num_map_points > self.num_min_triangulated_points out.pts = pts3d[mask_points] out.kf_cur = kf_cur out.idxs_cur = idx_cur_inliers[mask_points] out.kf_ref = kf_ref out.idxs_ref = idx_ref_inliers[mask_points] # set scene median depth to equal desired_median_depth' desired_median_depth = Parameters.kInitializerDesiredMedianDepth median_depth = kf_cur.compute_points_median_depth(out.pts) depth_scale = desired_median_depth / median_depth print('forcing current median depth ', median_depth, ' to ', desired_median_depth) out.pts[:, :3] = out.pts[:, :3] * depth_scale # scale points tcw = kf_cur.tcw * depth_scale # scale initial baseline kf_cur.update_translation(tcw) map.delete() if is_ok: Printer.green('Inializer: ok!') else: self.num_failures += 1 #Printer.red('Inializer: ko!') print('|------------') return out, is_ok