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
