def find_matches_along_line(f, e, line, descriptor, radius = parameters.kMaxReprojectionDistance, overlap_ratio = 0.5): #print('line: ', line[0], ", ", line[1]) step = radius*(1-overlap_ratio) delta = line[1].ravel() - line[0].ravel() length = np.linalg.norm(delta) n = max( int(math.ceil(length/step)), 1) delta = delta/n #print('delta: ', delta) best_dist = math.inf best_dist2 = math.inf best_k_idx = -1 for i in range(n+1): x = line[0].ravel() + i*delta for k_idx in f.kd.query_ball_point(x, radius): # if no point associated if f.points[k_idx] is None: descriptor_dist = Frame.descriptor_distance(f.des[k_idx], descriptor) if descriptor_dist < parameters.kMaxDescriptorDistanceSearchEpipolar: #return k_idx # stop at the first match if descriptor_dist < best_dist: best_dist2 = best_dist best_dist = descriptor_dist best_k_idx = k_idx # N.B.: the search "segment line" can have a large width => it is better to use the match distance ratio test if best_dist < best_dist2 * parameters.kMatchRatioTest: return best_k_idx, best_dist else: return -1, 0
def min_des_distance(self, descriptor): return min( [Frame.descriptor_distance(d, descriptor) for d in self.des()])
def min_des_distance(self, descriptor): with self._lock_features: #return min([Frame.descriptor_distance(d, descriptor) for d in self.descriptors()]) return Frame.descriptor_distance(self.des, descriptor)
def search_frame_for_triangulation(kf1, kf2, idxs1=None, idxs2=None, max_descriptor_distance=0.5*Parameters.kMaxDescriptorDistance): idxs2_out = [] idxs1_out = [] num_found_matches = 0 img2_epi = None if __debug__: timer = Timer() timer.start() O1w = kf1.Ow O2w = kf2.Ow # compute epipoles e1,_ = kf1.project_point(O2w) # in first frame e2,_ = kf2.project_point(O1w) # in second frame #print('e1: ', e1) #print('e2: ', e2) baseline = np.linalg.norm(O1w-O2w) # if the translation is too small we cannot triangulate # if baseline < Parameters.kMinTraslation: # we assume the Inializer has been used for building the first map # Printer.red("search for triangulation: impossible with almost zero translation!") # return idxs1_out, idxs2_out, num_found_matches, img2_epi # EXIT # else: medianDepth = kf2.compute_points_median_depth() if medianDepth == -1: Printer.orange("search for triangulation: f2 with no points") medianDepth = kf1.compute_points_median_depth() ratioBaselineDepth = baseline/medianDepth if ratioBaselineDepth < Parameters.kMinRatioBaselineDepth: Printer.orange("search for triangulation: impossible with too low ratioBaselineDepth!") return idxs1_out, idxs2_out, num_found_matches, img2_epi # EXIT # compute the fundamental matrix between the two frames by using their estimated poses F12, H21 = computeF12(kf1, kf2) if idxs1 is None or idxs2 is None: timerMatch = Timer() timerMatch.start() idxs1, idxs2 = Frame.feature_matcher.match(kf1.des, kf2.des) #print('search_frame_for_triangulation - matching - timer: ', timerMatch.elapsed()) rot_histo = RotationHistogram() check_orientation = kCheckFeaturesOrientation and Frame.oriented_features # check epipolar constraints for i1,i2 in zip(idxs1,idxs2): if kf1.get_point_match(i1) is not None or kf2.get_point_match(i2) is not None: # we are searching for keypoint matches where both keypoints do not have a corresponding map point #print('existing point on match') continue descriptor_dist = Frame.descriptor_distance(kf1.des[i1], kf2.des[i2]) if descriptor_dist > max_descriptor_distance: continue kp1 = kf1.kpsu[i1] #kp1_scale_factor = Frame.feature_manager.scale_factors[kf1.octaves[i1]] #kp1_size = f1.sizes[i1] # discard points which are too close to the epipole #if np.linalg.norm(kp1-e1) < Parameters.kMinDistanceFromEpipole * kp1_scale_factor: #if np.linalg.norm(kp1-e1) - kp1_size < Parameters.kMinDistanceFromEpipole: # N.B.: this is too much conservative => it filters too much # continue kp2 = kf2.kpsu[i2] kp2_scale_factor = Frame.feature_manager.scale_factors[kf2.octaves[i2]] # kp2_size = f2.sizes[i2] # discard points which are too close to the epipole delta = kp2-e2 #if np.linalg.norm(delta) < Parameters.kMinDistanceFromEpipole * kp2_scale_factor: if np.inner(delta,delta) < kMinDistanceFromEpipole2 * kp2_scale_factor: # OR. # #if np.linalg.norm(delta) - kp2_size < Parameters.kMinDistanceFromEpipole: # N.B.: this is too much conservative => it filters too much continue # check epipolar constraint sigma2_kp2 = Frame.feature_manager.level_sigmas2[kf2.octaves[i2]] if check_dist_epipolar_line(kp1,kp2,F12,sigma2_kp2): idxs1_out.append(i1) idxs2_out.append(i2) if check_orientation: index_match = len(idxs1_out)-1 rot = kf1.angles[i1]-kf2.angles[i2] rot_histo.push(rot,index_match) #else: # print('discarding point match non respecting epipolar constraint') if check_orientation: valid_match_idxs = rot_histo.get_valid_idxs() #print('checking orientation consistency - valid matches % :', len(valid_match_idxs)/max(1,len(idxs1_out))*100,'% of ', len(idxs1_out),'matches') #print('rotation histogram: ', rot_histo) idxs1_out = np.array(idxs1_out)[valid_match_idxs] idxs2_out = np.array(idxs2_out)[valid_match_idxs] num_found_matches = len(idxs1_out) #if __debug__: #print('search_frame_for_triangulation - timer: ', timer.elapsed()) return idxs1_out, idxs2_out, num_found_matches, img2_epi