def __init__(self, system):
if kDebugDrawMatches:
Frame.is_store_imgs = True
self.system = system
self.camera = system.camera
self.map = system.map
self.local_mapping = system.local_mapping
self.intializer = Initializer()
self.motion_model = MotionModel(
) # motion model for current frame pose prediction without damping
#self.motion_model = MotionModelDamping() # motion model for current frame pose prediction with damping
self.dyn_config = SLAMDynamicConfig()
self.descriptor_distance_sigma = Parameters.kMaxDescriptorDistance
self.reproj_err_frame_map_sigma = Parameters.kMaxReprojectionDistanceMap
self.max_frames_between_kfs = int(system.camera.fps)
self.min_frames_between_kfs = 0
self.state = SlamState.NO_IMAGES_YET
self.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of matched points
self.num_matched_map_points = None # current number of matched map points (matched and found valid in current pose optimization)
self.num_kf_ref_tracked_points = None # number of tracked points in k_ref (considering a minimum number of observations)
self.mask_match = None
self.pose_is_ok = False
self.predicted_pose = None
self.velocity = None
self.f_cur = None
self.idxs_cur = None
self.f_ref = None
self.idxs_ref = None
self.kf_ref = None # reference keyframe (in general, different from last keyframe depending on the used approach)
self.kf_last = None # last keyframe
self.kid_last_BA = -1 # last keyframe id when performed BA
self.local_keyframes = [] # local keyframes
self.local_points = [] # local points
self.tracking_history = TrackingHistory()
self.timer_verbose = kTimerVerbose # set this to True if you want to print timings
self.timer_main_track = TimerFps('Track',
is_verbose=self.timer_verbose)
self.timer_pose_opt = TimerFps('Pose optimization',
is_verbose=self.timer_verbose)
self.timer_seach_frame_proj = TimerFps('Search frame by proj',
is_verbose=self.timer_verbose)
self.timer_match = TimerFps('Match', is_verbose=self.timer_verbose)
self.timer_pose_est = TimerFps('Ess mat pose estimation',
is_verbose=self.timer_verbose)
self.timer_frame = TimerFps('Frame', is_verbose=self.timer_verbose)
self.timer_seach_map = TimerFps('Search map',
is_verbose=self.timer_verbose)
self.init_history = True
self.poses = [] # history of poses
self.t0_est = None # history of estimated translations
self.t0_gt = None # history of ground truth translations (if available)
self.traj3d_est = [
] # history of estimated translations centered w.r.t. first one
self.traj3d_gt = [
] # history of estimated ground truth translations centered w.r.t. first one
self.cur_R = None # current rotation w.r.t. world frame
self.cur_t = None # current translation w.r.t. world frame
self.trueX, self.trueY, self.trueZ = None, None, None
self.groundtruth = system.groundtruth # not actually used here; could be used for evaluating performances
if kLogKFinfoToFile:
self.kf_info_logger = Logging.setup_file_logger(
'kf_info_logger',
'kf_info.log',
formatter=Logging.simple_log_formatter)
class Tracking(object):
def __init__(self, system):
if kDebugDrawMatches:
Frame.is_store_imgs = True
self.system = system
self.camera = system.camera
self.map = system.map
self.local_mapping = system.local_mapping
self.intializer = Initializer()
self.motion_model = MotionModel(
) # motion model for current frame pose prediction without damping
#self.motion_model = MotionModelDamping() # motion model for current frame pose prediction with damping
self.dyn_config = SLAMDynamicConfig()
self.descriptor_distance_sigma = Parameters.kMaxDescriptorDistance
self.reproj_err_frame_map_sigma = Parameters.kMaxReprojectionDistanceMap
self.max_frames_between_kfs = int(system.camera.fps)
self.min_frames_between_kfs = 0
self.state = SlamState.NO_IMAGES_YET
self.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of matched points
self.num_matched_map_points = None # current number of matched map points (matched and found valid in current pose optimization)
self.num_kf_ref_tracked_points = None # number of tracked points in k_ref (considering a minimum number of observations)
self.mask_match = None
self.pose_is_ok = False
self.predicted_pose = None
self.velocity = None
self.f_cur = None
self.idxs_cur = None
self.f_ref = None
self.idxs_ref = None
self.kf_ref = None # reference keyframe (in general, different from last keyframe depending on the used approach)
self.kf_last = None # last keyframe
self.kid_last_BA = -1 # last keyframe id when performed BA
self.local_keyframes = [] # local keyframes
self.local_points = [] # local points
self.tracking_history = TrackingHistory()
self.timer_verbose = kTimerVerbose # set this to True if you want to print timings
self.timer_main_track = TimerFps('Track',
is_verbose=self.timer_verbose)
self.timer_pose_opt = TimerFps('Pose optimization',
is_verbose=self.timer_verbose)
self.timer_seach_frame_proj = TimerFps('Search frame by proj',
is_verbose=self.timer_verbose)
self.timer_match = TimerFps('Match', is_verbose=self.timer_verbose)
self.timer_pose_est = TimerFps('Ess mat pose estimation',
is_verbose=self.timer_verbose)
self.timer_frame = TimerFps('Frame', is_verbose=self.timer_verbose)
self.timer_seach_map = TimerFps('Search map',
is_verbose=self.timer_verbose)
self.init_history = True
self.poses = [] # history of poses
self.t0_est = None # history of estimated translations
self.t0_gt = None # history of ground truth translations (if available)
self.traj3d_est = [
] # history of estimated translations centered w.r.t. first one
self.traj3d_gt = [
] # history of estimated ground truth translations centered w.r.t. first one
self.cur_R = None # current rotation w.r.t. world frame
self.cur_t = None # current translation w.r.t. world frame
self.trueX, self.trueY, self.trueZ = None, None, None
self.groundtruth = system.groundtruth # not actually used here; could be used for evaluating performances
if kLogKFinfoToFile:
self.kf_info_logger = Logging.setup_file_logger(
'kf_info_logger',
'kf_info.log',
formatter=Logging.simple_log_formatter)
# estimate a pose from a fitted essential mat;
# since we do not have an interframe translation scale, this fitting can be used to detect outliers, estimate interframe orientation and translation direction
# N.B. read the NBs of the method estimate_pose_ess_mat(), where the limitations of this method are explained
def estimate_pose_by_fitting_ess_mat(self, f_ref, f_cur, idxs_ref,
idxs_cur):
# N.B.: in order to understand the limitations of fitting an essential mat, read the comments of the method self.estimate_pose_ess_mat()
self.timer_pose_est.start()
# estimate inter frame camera motion by using found keypoint matches
# output of the following function is: Trc = [Rrc, trc] with ||trc||=1 where c=cur, r=ref and pr = Trc * pc
Mrc, self.mask_match = estimate_pose_ess_mat(
f_ref.kpsn[idxs_ref],
f_cur.kpsn[idxs_cur],
method=cv2.RANSAC,
prob=kRansacProb,
threshold=kRansacThresholdNormalized)
#Mcr = np.linalg.inv(poseRt(Mrc[:3, :3], Mrc[:3, 3]))
Mcr = inv_T(Mrc)
estimated_Tcw = np.dot(Mcr, f_ref.pose)
self.timer_pose_est.refresh()
# 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('# inliers: ', self.num_inliers)
idxs_ref = idxs_ref[mask_idxs]
idxs_cur = idxs_cur[mask_idxs]
# if there are not enough inliers do not use the estimated pose
if self.num_inliers < kNumMinInliersEssentialMat:
#f_cur.update_pose(f_ref.pose) # reset estimated pose to previous frame
Printer.red('Essential mat: not enough inliers!')
else:
# use the estimated pose as an initial guess for the subsequent pose optimization
# set only the estimated rotation (essential mat computation does not provide a scale for the translation, see above)
#f_cur.pose[:3,:3] = estimated_Tcw[:3,:3] # copy only the rotation
#f_cur.pose[:,3] = f_ref.pose[:,3].copy() # override translation with ref frame translation
Rcw = estimated_Tcw[:3, :3] # copy only the rotation
tcw = f_ref.pose[:3,
3] # override translation with ref frame translation
f_cur.update_rotation_and_translation(Rcw, tcw)
return idxs_ref, idxs_cur
def pose_optimization(self, f_cur, name=''):
print('pose opt %s ' % (name))
pose_before = f_cur.pose.copy()
# f_cur pose optimization 1 (here we use f_cur pose as first guess and exploit the matched map points of f_ref )
self.timer_pose_opt.start()
pose_opt_error, self.pose_is_ok, self.num_matched_map_points = optimizer_g2o.pose_optimization(
f_cur, verbose=False)
self.timer_pose_opt.pause()
print(' error^2: %f, ok: %d' %
(pose_opt_error, int(self.pose_is_ok)))
if not self.pose_is_ok:
# if current pose optimization failed, reset f_cur pose
f_cur.update_pose(pose_before)
return self.pose_is_ok
# track camera motion of f_cur w.r.t. f_ref
def track_previous_frame(self, f_ref, f_cur):
print('>>>> tracking previous frame ...')
is_search_frame_by_projection_failure = False
use_search_frame_by_projection = self.motion_model.is_ok and kUseSearchFrameByProjection and kUseMotionModel
if use_search_frame_by_projection:
# search frame by projection: match map points observed in f_ref with keypoints of f_cur
print('search frame by projection')
search_radius = Parameters.kMaxReprojectionDistanceFrame
f_cur.reset_points()
self.timer_seach_frame_proj.start()
idxs_ref, idxs_cur, num_found_map_pts = search_frame_by_projection(
f_ref,
f_cur,
max_reproj_distance=search_radius,
max_descriptor_distance=self.descriptor_distance_sigma)
self.timer_seach_frame_proj.refresh()
self.num_matched_kps = len(idxs_cur)
print("# matched map points in prev frame: %d " %
self.num_matched_kps)
# if not enough map point matches consider a larger search radius
if self.num_matched_kps < Parameters.kMinNumMatchedFeaturesSearchFrameByProjection:
f_cur.remove_frame_views(idxs_cur)
f_cur.reset_points()
idxs_ref, idxs_cur, num_found_map_pts = search_frame_by_projection(
f_ref,
f_cur,
max_reproj_distance=2 * search_radius,
max_descriptor_distance=0.5 *
self.descriptor_distance_sigma)
self.num_matched_kps = len(idxs_cur)
Printer.orange(
"# matched map points in prev frame (wider search): %d " %
self.num_matched_kps)
if kDebugDrawMatches and True:
img_matches = draw_feature_matches(f_ref.img,
f_cur.img,
f_ref.kps[idxs_ref],
f_cur.kps[idxs_cur],
f_ref.sizes[idxs_ref],
f_cur.sizes[idxs_cur],
horizontal=False)
cv2.imshow('tracking frame by projection - matches',
img_matches)
cv2.waitKey(1)
if self.num_matched_kps < Parameters.kMinNumMatchedFeaturesSearchFrameByProjection:
f_cur.remove_frame_views(idxs_cur)
f_cur.reset_points()
is_search_frame_by_projection_failure = True
Printer.red(
'Not enough matches in search frame by projection: ',
self.num_matched_kps)
else:
# search frame by projection was successful
if kUseDynamicDesDistanceTh:
self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat(
f_ref, f_cur, idxs_ref, idxs_cur)
# store tracking info (for possible reuse)
self.idxs_ref = idxs_ref
self.idxs_cur = idxs_cur
# f_cur pose optimization 1:
# here, we use f_cur pose as first guess and exploit the matched map point of f_ref
self.pose_optimization(f_cur, 'proj-frame-frame')
# update matched map points; discard outliers detected in last pose optimization
num_matched_points = f_cur.clean_outlier_map_points()
print(' # num_matched_map_points: %d' %
(self.num_matched_map_points))
#print(' # matched points: %d' % (num_matched_points) )
if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackFrame:
Printer.red(
'failure in tracking previous frame, # matched map points: ',
self.num_matched_map_points)
self.pose_is_ok = False
if not use_search_frame_by_projection or is_search_frame_by_projection_failure:
self.track_reference_frame(f_ref, f_cur, 'match-frame-frame')
# track camera motion of f_cur w.r.t. f_ref
# estimate motion by matching keypoint descriptors
def track_reference_frame(self, f_ref, f_cur, name=''):
print('>>>> tracking reference %d ...' % (f_ref.id))
if f_ref is None:
return
# find keypoint matches between f_cur and kf_ref
print('matching keypoints with ', Frame.feature_matcher.type.name)
self.timer_match.start()
idxs_cur, idxs_ref = match_frames(f_cur, f_ref)
self.timer_match.refresh()
self.num_matched_kps = idxs_cur.shape[0]
print("# keypoints matched: %d " % self.num_matched_kps)
if kUseEssentialMatrixFitting:
# estimate camera orientation and inlier matches by fitting and essential matrix (see the limitations above)
idxs_ref, idxs_cur = self.estimate_pose_by_fitting_ess_mat(
f_ref, f_cur, idxs_ref, idxs_cur)
if kUseDynamicDesDistanceTh:
self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat(
f_ref, f_cur, idxs_ref, idxs_cur)
# propagate map point matches from kf_ref to f_cur (do not override idxs_ref, idxs_cur)
num_found_map_pts_inter_frame, idx_ref_prop, idx_cur_prop = propagate_map_point_matches(
f_ref,
f_cur,
idxs_ref,
idxs_cur,
max_descriptor_distance=self.descriptor_distance_sigma)
print("# matched map points in prev frame: %d " %
num_found_map_pts_inter_frame)
if kDebugDrawMatches and True:
img_matches = draw_feature_matches(f_ref.img,
f_cur.img,
f_ref.kps[idx_ref_prop],
f_cur.kps[idx_cur_prop],
f_ref.sizes[idx_ref_prop],
f_cur.sizes[idx_cur_prop],
horizontal=False)
cv2.imshow('tracking frame (no projection) - matches', img_matches)
cv2.waitKey(1)
# store tracking info (for possible reuse)
self.idxs_ref = idxs_ref
self.idxs_cur = idxs_cur
# f_cur pose optimization using last matches with kf_ref:
# here, we use first guess of f_cur pose and propated map point matches from f_ref (matched keypoints)
self.pose_optimization(f_cur, name)
# update matched map points; discard outliers detected in last pose optimization
num_matched_points = f_cur.clean_outlier_map_points()
print(' # num_matched_map_points: %d' %
(self.num_matched_map_points))
#print(' # matched points: %d' % (num_matched_points) )
if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackFrame:
f_cur.remove_frame_views(idxs_cur)
f_cur.reset_points()
Printer.red(
'failure in tracking reference %d, # matched map points: %d' %
(f_ref.id, self.num_matched_map_points))
self.pose_is_ok = False
# track camera motion of f_cur w.r.t. given keyframe
# estimate motion by matching keypoint descriptors
def track_keyframe(self, keyframe, f_cur, name='match-frame-keyframe'):
f_cur.update_pose(self.f_ref.pose.copy()
) # start pose optimization from last frame pose
self.track_reference_frame(keyframe, f_cur, name)
def update_local_map(self):
self.f_cur.clean_bad_map_points()
#self.local_points = self.map.local_map.get_points()
self.kf_ref, self.local_keyframes, self.local_points = self.map.local_map.get_frame_covisibles(
self.f_cur)
self.f_cur.kf_ref = self.kf_ref
# track camera motion of f_cur w.r.t. the built local map
# find matches between {local map points} (points in the built local map) and {unmatched keypoints of f_cur}
def track_local_map(self, f_cur):
if self.map.local_map.is_empty():
return
print('>>>> tracking local map...')
self.timer_seach_map.start()
self.update_local_map()
num_found_map_pts, reproj_err_frame_map_sigma, matched_points_frame_idxs = search_map_by_projection(
self.local_points,
f_cur,
max_reproj_distance=self.
reproj_err_frame_map_sigma, #Parameters.kMaxReprojectionDistanceMap,
max_descriptor_distance=self.descriptor_distance_sigma,
ratio_test=Parameters.kMatchRatioTestMap
) # use the updated local map
self.timer_seach_map.refresh()
#print('reproj_err_sigma: ', reproj_err_frame_map_sigma, ' used: ', self.reproj_err_frame_map_sigma)
print("# new matched map points in local map: %d " % num_found_map_pts)
print("# local map points ", self.map.local_map.num_points())
if kDebugDrawMatches and True:
img_matched_trails = f_cur.draw_feature_trails(
f_cur.img.copy(),
matched_points_frame_idxs,
trail_max_length=3)
cv2.imshow('tracking local map - matched trails',
img_matched_trails)
cv2.waitKey(1)
# f_cur pose optimization 2 with all the matched local map points
self.pose_optimization(f_cur, 'proj-map-frame')
f_cur.update_map_points_statistics(
) # here we do not reset outliers; we let them reach the keyframe generation
# and then bundle adjustment will possible decide if remove them or not;
# only after keyframe generation the outliers are cleaned!
print(' # num_matched_map_points: %d' %
(self.num_matched_map_points))
if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackLocalMap:
Printer.red(
'failure in tracking local map, # matched map points: ',
self.num_matched_map_points)
self.pose_is_ok = False
#if kUseDynamicDesDistanceTh:
# self.reproj_err_frame_map_sigma = self.dyn_config.update_reproj_err_map_stat(reproj_err_frame_map_sigma)
# store frame history in order to retrieve the complete camera trajectory
def update_tracking_history(self):
if self.state == SlamState.OK:
isometry3d_Tcr = self.f_cur.isometry3d * self.f_cur.kf_ref.isometry3d.inverse(
) # pose of current frame w.r.t. current reference keyframe kf_ref
self.tracking_history.relative_frame_poses.append(isometry3d_Tcr)
self.tracking_history.kf_references.append(self.kf_ref)
self.tracking_history.timestamps.append(self.f_cur.timestamp)
else:
self.tracking_history.relative_frame_poses.append(
self.tracking_history.relative_frame_poses[-1])
self.tracking_history.kf_references.append(
self.tracking_history.kf_references[-1])
self.tracking_history.timestamps.append(
self.tracking_history.timestamps[-1])
self.tracking_history.slam_states.append(self.state)
def need_new_keyframe(self, f_cur):
num_keyframes = self.map.num_keyframes()
nMinObs = kNumMinObsForKeyFrameDefault
if num_keyframes <= 2:
nMinObs = 2 # if just two keyframes then we can have just two observations
num_kf_ref_tracked_points = self.kf_ref.num_tracked_points(
nMinObs) # number of tracked points in k_ref
num_f_cur_tracked_points = f_cur.num_matched_inlier_map_points(
) # number of inliers in f_cur
Printer.purple('F(%d) #points: %d, KF(%d) #points: %d ' %
(f_cur.id, num_f_cur_tracked_points, self.kf_ref.id,
num_kf_ref_tracked_points))
if kLogKFinfoToFile:
self.kf_info_logger.info(
'F(%d) #points: %d, KF(%d) #points: %d ' %
(f_cur.id, num_f_cur_tracked_points, self.kf_ref.id,
num_kf_ref_tracked_points))
self.num_kf_ref_tracked_points = num_kf_ref_tracked_points
is_local_mapping_idle = self.local_mapping.is_idle()
local_mapping_queue_size = self.local_mapping.queue_size()
print('is_local_mapping_idle: ', is_local_mapping_idle,
', local_mapping_queue_size: ', local_mapping_queue_size)
# condition 1: more than "max_frames_between_kfs" have passed from last keyframe insertion
cond1 = f_cur.id >= (self.kf_last.id + self.max_frames_between_kfs)
# condition 2: more than "min_frames_between_kfs" have passed and local mapping is idle
cond2 = (f_cur.id >=
(self.kf_last.id +
self.min_frames_between_kfs)) & is_local_mapping_idle
#cond2 = (f_cur.id >= (self.kf_last.id + self.min_frames_between_kfs))
# condition 3: few tracked features compared to reference keyframe
cond3 = (num_f_cur_tracked_points <
num_kf_ref_tracked_points * Parameters.kThNewKfRefRatio) and (
num_f_cur_tracked_points >
Parameters.kNumMinPointsForNewKf)
#print('KF conditions: %d %d %d' % (cond1, cond2, cond3) )
ret = (cond1 or cond2) and cond3
if ret:
if is_local_mapping_idle:
return True
else:
self.local_mapping.interrupt_optimization()
if True:
if local_mapping_queue_size <= 3:
return True
else:
return False
else:
return False
else:
return False
# @ main track method @
def track(self, img, frame_id, timestamp=None):
Printer.cyan('@tracking')
time_start = time.time()
# check image size is coherent with camera params
print("img.shape: ", img.shape)
print("camera ", self.camera.height, " x ", self.camera.width)
assert img.shape[0:2] == (self.camera.height, self.camera.width)
if timestamp is not None:
print('timestamp: ', timestamp)
self.timer_main_track.start()
# build current frame
self.timer_frame.start()
f_cur = Frame(img, self.camera, timestamp=timestamp)
self.f_cur = f_cur
print("frame: ", f_cur.id)
self.timer_frame.refresh()
# reset indexes of matches
self.idxs_ref = []
self.idxs_cur = []
if self.state == SlamState.NO_IMAGES_YET:
# push first frame in the inizializer
self.intializer.init(f_cur)
self.state = SlamState.NOT_INITIALIZED
return # EXIT (jump to second frame)
if self.state == SlamState.NOT_INITIALIZED:
# try to inizialize
initializer_output, intializer_is_ok = self.intializer.initialize(
f_cur, img)
if intializer_is_ok:
kf_ref = initializer_output.kf_ref
kf_cur = initializer_output.kf_cur
# add the two initialized frames in the map
self.map.add_frame(
kf_ref) # add first frame in map and update its frame id
self.map.add_frame(
kf_cur) # add second frame in map and update its frame id
# add the two initialized frames as keyframes in the map
self.map.add_keyframe(
kf_ref) # add first keyframe in map and update its kid
self.map.add_keyframe(
kf_cur) # add second keyframe in map and update its kid
kf_ref.init_observations()
kf_cur.init_observations()
# add points in map
new_pts_count, _, _ = self.map.add_points(
initializer_output.pts,
None,
kf_cur,
kf_ref,
initializer_output.idxs_cur,
initializer_output.idxs_ref,
img,
do_check=False)
Printer.green("map: initialized %d new points" %
(new_pts_count))
# update covisibility graph connections
kf_ref.update_connections()
kf_cur.update_connections()
# update tracking info
self.f_cur = kf_cur
self.f_cur.kf_ref = kf_ref
self.kf_ref = kf_cur # set reference keyframe
self.kf_last = kf_cur # set last added keyframe
self.map.local_map.update(self.kf_ref)
self.state = SlamState.OK
self.update_tracking_history()
self.motion_model.update_pose(kf_cur.timestamp,
kf_cur.position,
kf_cur.quaternion)
self.motion_model.is_ok = False # after initialization you cannot use motion model for next frame pose prediction (time ids of initialized poses may not be consecutive)
self.intializer.reset()
if kUseDynamicDesDistanceTh:
self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat(
kf_ref, kf_cur, initializer_output.idxs_ref,
initializer_output.idxs_cur)
return # EXIT (jump to next frame)
# get previous frame in map as reference
f_ref = self.map.get_frame(-1)
#f_ref_2 = self.map.get_frame(-2)
self.f_ref = f_ref
# add current frame f_cur to map
self.map.add_frame(f_cur)
self.f_cur.kf_ref = self.kf_ref
# reset pose state flag
self.pose_is_ok = False
with self.map.update_lock:
# check for map point replacements in previous frame f_ref (some points might have been replaced by local mapping during point fusion)
self.f_ref.check_replaced_map_points()
if kUseDynamicDesDistanceTh:
print('descriptor_distance_sigma: ',
self.descriptor_distance_sigma)
self.local_mapping.descriptor_distance_sigma = self.descriptor_distance_sigma
# udpdate (velocity) old motion model # c1=ref_ref, c2=ref, c3=cur; c=cur, r=ref
#self.velocity = np.dot(f_ref.pose, inv_T(f_ref_2.pose)) # Tc2c1 = Tc2w * Twc1 (predicted Tcr)
#self.predicted_pose = g2o.Isometry3d(np.dot(self.velocity, f_ref.pose)) # Tc3w = Tc2c1 * Tc2w (predicted Tcw)
# set intial guess for current pose optimization
if kUseMotionModel and self.motion_model.is_ok:
print('using motion model for next pose prediction')
# update f_ref pose according to its reference keyframe (the pose of the reference keyframe could be updated by local mapping)
self.f_ref.update_pose(
self.tracking_history.relative_frame_poses[-1] *
self.f_ref.kf_ref.isometry3d)
# predict pose by using motion model
self.predicted_pose, _ = self.motion_model.predict_pose(
timestamp, self.f_ref.position, self.f_ref.orientation)
f_cur.update_pose(self.predicted_pose)
else:
print('setting f_cur.pose <-- f_ref.pose')
# use reference frame pose as initial guess
f_cur.update_pose(f_ref.pose)
# track camera motion from f_ref to f_cur
self.track_previous_frame(f_ref, f_cur)
if not self.pose_is_ok:
# if previous track didn't go well then track the camera motion from kf_ref to f_cur
self.track_keyframe(self.kf_ref, f_cur)
# now, having a better estimate of f_cur pose, we can find more map point matches:
# find matches between {local map points} (points in the local map) and {unmatched keypoints of f_cur}
if self.pose_is_ok:
self.track_local_map(f_cur)
# end block {with self.map.update_lock:}
# TODO: add relocalization
# HACK: since local mapping is not fast enough in python (and tracking is not in real-time) => give local mapping more time to process stuff
self.wait_for_local_mapping(
) # N.B.: this must be outside the `with self.map.update_lock:` block
with self.map.update_lock:
# update slam state
if self.pose_is_ok:
self.state = SlamState.OK
else:
self.state = SlamState.LOST
Printer.red('tracking failure')
# update motion model state
self.motion_model.is_ok = self.pose_is_ok
if self.pose_is_ok: # if tracking was successful
# update motion model
self.motion_model.update_pose(timestamp, f_cur.position,
f_cur.quaternion)
f_cur.clean_vo_map_points()
# do we need a new KeyFrame?
need_new_kf = self.need_new_keyframe(f_cur)
if need_new_kf:
Printer.green('adding new KF with frame id % d: ' %
(f_cur.id))
if kLogKFinfoToFile:
self.kf_info_logger.info(
'adding new KF with frame id % d: ' % (f_cur.id))
kf_new = KeyFrame(f_cur, img)
self.kf_last = kf_new
self.kf_ref = kf_new
f_cur.kf_ref = kf_new
self.local_mapping.push_keyframe(kf_new)
if not kLocalMappingOnSeparateThread:
self.local_mapping.do_local_mapping()
else:
Printer.yellow('NOT KF')
# From ORBSLAM2:
# Clean outliers once keyframe generation has been managed:
# we allow points with high innovation (considered outliers by the Huber Function)
# pass to the new keyframe, so that bundle adjustment will finally decide
# if they are outliers or not. We don't want next frame to estimate its position
# with those points so we discard them in the frame.
f_cur.clean_outlier_map_points()
if self.f_cur.kf_ref is None:
self.f_cur.kf_ref = self.kf_ref
self.update_tracking_history(
) # must stay after having updated slam state (self.state)
Printer.green("map: %d points, %d keyframes" %
(self.map.num_points(), self.map.num_keyframes()))
#self.update_history()
self.timer_main_track.refresh()
duration = time.time() - time_start
print('tracking duration: ', duration)
# Since we do not have real-time performances, we can slow-down things and make tracking wait till local mapping gets idle
# N.B.: this function must be called outside 'with self.map.update_lock' blocks,
# since both self.track() and the local-mapping optimization use the RLock 'map.update_lock'
# => they cannot wait for each other once map.update_lock is locked (deadlock)
def wait_for_local_mapping(self):
if kTrackingWaitForLocalMappingToGetIdle:
#while not self.local_mapping.is_idle() or self.local_mapping.queue_size()>0:
if not self.local_mapping.is_idle():
print('>>>> waiting for local mapping...')
self.local_mapping.wait_idle()
else:
if not self.local_mapping.is_idle(
) and kTrackingWaitForLocalMappingSleepTime > 0:
print('>>>> sleeping for local mapping...')
time.sleep(kTrackingWaitForLocalMappingSleepTime)