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