def search_and_fuse(points, keyframe,
max_reproj_distance=Parameters.kMaxReprojectionDistanceFuse,
max_descriptor_distance = 0.5*Parameters.kMaxDescriptorDistance,
ratio_test=Parameters.kMatchRatioTestMap):
#max_descriptor_distance = 0.5 * Parameters.kMaxDescriptorDistance
fused_pts_count = 0
Ow = keyframe.Ow
if len(points) == 0:
Printer.red('search_and_fuse - no points')
return
# get all matched points of keyframe
good_pts_idxs = np.flatnonzero(points!=None)
good_pts = points[good_pts_idxs]
if len(good_pts_idxs) == 0:
Printer.red('search_and_fuse - no matched points')
return
# check if points are visible
good_pts_visible, good_projs, good_depths, good_dists = keyframe.are_visible(good_pts)
if len(good_pts_visible) == 0:
Printer.red('search_and_fuse - no visible points')
return
predicted_levels = predict_detection_levels(good_pts, good_dists)
kp_scale_factors = Frame.feature_manager.scale_factors[predicted_levels]
radiuses = max_reproj_distance * kp_scale_factors
kd_idxs = keyframe.kd.query_ball_point(good_projs, radiuses)
#for i, p in enumerate(points):
for i,p,j in zip(good_pts_idxs,good_pts,range(len(good_pts))):
if not good_pts_visible[j] or p.is_bad: # point not visible in frame or point is bad
#print('p[%d] visible: %d, bad: %d' % (i, int(good_pts_visible[j]), int(p.is_bad)))
continue
if p.is_in_keyframe(keyframe): # we already matched this map point to this keyframe
#print('p[%d] already in keyframe' % (i))
continue
# predicted_level = p.predict_detection_level(good_dists[j])
# kp_scale_factor = Frame.feature_manager.scale_factors[predicted_level]
# radius = max_reproj_distance * kp_scale_factor
predicted_level = predicted_levels[j]
#print('p[%d] radius: %f' % (i,radius))
best_dist = math.inf
best_dist2 = math.inf
best_level = -1
best_level2 = -1
best_kd_idx = -1
# find closest keypoints of frame
proj = good_projs[j]
#for kd_idx in keyframe.kd.query_ball_point(proj, radius):
for kd_idx in kd_idxs[j]:
# check detection level
kp_level = keyframe.octaves[kd_idx]
if (kp_level<predicted_level-1) or (kp_level>predicted_level):
#print('p[%d] wrong predicted level **********************************' % (i))
continue
# check the reprojection error
kp = keyframe.kpsu[kd_idx]
invSigma2 = Frame.feature_manager.inv_level_sigmas2[kp_level]
err = proj - kp
chi2 = np.inner(err,err)*invSigma2
if chi2 > Parameters.kChi2Mono: # chi-square 2 DOFs (Hartley Zisserman pg 119)
#print('p[%d] big reproj err %f **********************************' % (i,chi2))
continue
descriptor_dist = p.min_des_distance(keyframe.des[kd_idx])
#print('p[%d] descriptor_dist %f **********************************' % (i,descriptor_dist))
#if descriptor_dist < max_descriptor_distance and descriptor_dist < best_dist:
if descriptor_dist < best_dist:
best_dist2 = best_dist
best_level2 = best_level
best_dist = descriptor_dist
best_level = kp_level
best_kd_idx = kd_idx
else:
if descriptor_dist < best_dist2: # N.O.
best_dist2 = descriptor_dist
best_level2 = kp_level
#if best_kd_idx > -1 and best_dist < max_descriptor_distance:
if best_dist < max_descriptor_distance:
# apply match distance ratio test only if the best and second are in the same scale level
if (best_level2 == best_level) and (best_dist > best_dist2 * ratio_test): # N.O.
#print('p[%d] best_dist > best_dist2 * ratio_test **********************************' % (i))
continue
p_keyframe = keyframe.get_point_match(best_kd_idx)
# if there is already a map point replace it otherwise add a new point
if p_keyframe is not None:
if not p_keyframe.is_bad:
if p_keyframe.num_observations > p.num_observations:
p.replace_with(p_keyframe)
else:
p_keyframe.replace_with(p)
else:
p.add_observation(keyframe, best_kd_idx)
#p.update_info() # done outside!
fused_pts_count += 1
return fused_pts_count
def search_map_by_projection(points, f_cur,
max_reproj_distance=Parameters.kMaxReprojectionDistanceMap,
max_descriptor_distance=Parameters.kMaxDescriptorDistance,
ratio_test=Parameters.kMatchRatioTestMap):
Ow = f_cur.Ow
found_pts_count = 0
found_pts_fidxs = [] # idx of matched points in current frame
#reproj_dists = []
if len(points) == 0:
return 0
# check if points are visible
visible_pts, projs, depths, dists = f_cur.are_visible(points)
predicted_levels = predict_detection_levels(points, dists)
kp_scale_factors = Frame.feature_manager.scale_factors[predicted_levels]
radiuses = max_reproj_distance * kp_scale_factors
kd_idxs = f_cur.kd.query_ball_point(projs, radiuses)
for i, p in enumerate(points):
if not visible_pts[i] or p.is_bad: # point not visible in frame or is bad
continue
if p.last_frame_id_seen == f_cur.id: # we already matched this map point to current frame or it was outlier
continue
p.increase_visible()
# predicted_level = p.predict_detection_level(dists[i])
predicted_level = predicted_levels[i]
# kp_scale_factor = Frame.feature_manager.scale_factors[predicted_level]
# radius = max_reproj_distance * kp_scale_factor
best_dist = math.inf
best_dist2 = math.inf
best_level = -1
best_level2 = -1
best_k_idx = -1
# find closest keypoints of f_cur
#for kd_idx in f_cur.kd.query_ball_point(projs[i], radius):
#for kd_idx in f_cur.kd.query_ball_point(proj, radius):
for kd_idx in kd_idxs[i]:
p_f = f_cur.points[kd_idx]
# check there is not already a match
if p_f is not None:
if p_f.num_observations > 0:
continue
# check detection level
kp_level = f_cur.octaves[kd_idx]
if (kp_level<predicted_level-1) or (kp_level>predicted_level):
continue
descriptor_dist = p.min_des_distance(f_cur.des[kd_idx])
if descriptor_dist < best_dist:
best_dist2 = best_dist
best_level2 = best_level
best_dist = descriptor_dist
best_level = kp_level
best_k_idx = kd_idx
else:
if descriptor_dist < best_dist2:
best_dist2 = descriptor_dist
best_level2 = kp_level
#if best_k_idx > -1 and best_dist < max_descriptor_distance:
if best_dist < max_descriptor_distance:
# apply match distance ratio test only if the best and second are in the same scale level
if (best_level2 == best_level) and (best_dist > best_dist2 * ratio_test):
continue
#print('best des distance: ', best_dist, ", max dist: ", Parameters.kMaxDescriptorDistance)
if p.add_frame_view(f_cur, best_k_idx):
found_pts_count += 1
found_pts_fidxs.append(best_k_idx)
#reproj_dists.append(np.linalg.norm(projs[i] - f_cur.kpsu[best_k_idx]))
# if len(reproj_dists) > 1:
# reproj_dist_sigma = 1.4826 * np.median(reproj_dists)
# else:
reproj_dist_sigma = max_descriptor_distance
return found_pts_count, reproj_dist_sigma, found_pts_fidxs