def __init__(self, map):
self.map = map
self.recently_added_points = set()
self.kf_cur = None # current processed keyframe
self.kid_last_BA = -1 # last keyframe id when performed BA
self.descriptor_distance_sigma = Parameters.kMaxDescriptorDistance
self.timer_verbose = kTimerVerbose # set this to True if you want to print timings
self.timer_triangulation = TimerFps('Triangulation', is_verbose = self.timer_verbose)
self.timer_pts_culling = TimerFps('Culling points', is_verbose = self.timer_verbose)
self.timer_pts_fusion = TimerFps('Fusing points', is_verbose = self.timer_verbose)
self.time_local_opt = TimerFps('Local optimization', is_verbose = self.timer_verbose)
self.time_large_opt = TimerFps('Large window optimization', is_verbose = self.timer_verbose)
self.queue = Queue()
self.work_thread = Thread(target=self.run)
self.stop = False
self.lock_accept_keyframe = RLock()
self._is_idle = True
self.idle_codition = Condition()
self.opt_abort_flag = g2o.Flag(False)
self.log_file = None
self.thread_large_BA = None
def __init__(self, cam, groundtruth, feature_tracker):
self.stage = VoStage.NO_IMAGES_YET
self.cam = cam
self.cur_image = None # current image
self.prev_image = None # previous/reference image
self.kps_ref = None # reference keypoints
self.des_ref = None # refeference descriptors
self.kps_cur = None # current keypoints
self.des_cur = None # current descriptors
self.cur_R = np.eye(3, 3) # current rotation
self.cur_t = np.zeros((3, 1)) # current translation
self.trueX, self.trueY, self.trueZ = None, None, None
self.groundtruth = groundtruth
self.feature_tracker = feature_tracker
self.track_result = None
self.mask_match = None # mask of matched keypoints used for drawing
self.draw_img = None
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.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of inliers
self.timer_verbose = False # set this to True if you want to print timings
self.timer_main = TimerFps('VO', is_verbose=self.timer_verbose)
self.timer_pose_est = TimerFps('PoseEst',
is_verbose=self.timer_verbose)
self.timer_feat = TimerFps('Feature', is_verbose=self.timer_verbose)
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)
class VisualOdometry(object):
def __init__(self, cam, grountruth, feature_tracker):
self.stage = VoStage.NO_IMAGES_YET
self.cam = cam
self.cur_image = None # current image
self.prev_image = None # previous/reference image
self.kps_ref = None # reference keypoints
self.des_ref = None # refeference descriptors
self.kps_cur = None # current keypoints
self.des_cur = None # current descriptors
self.cur_R = np.eye(3,3) # current rotation
self.cur_t = np.zeros((3,1)) # current translation
self.trueX, self.trueY, self.trueZ = None, None, None
self.grountruth = grountruth
self.feature_tracker = feature_tracker
self.track_result = None
self.mask_match = None # mask of matched keypoints used for drawing
self.draw_img = None
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.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of inliers
self.timer_verbose = False # set this to True if you want to print timings
self.timer_main = TimerFps('VO', is_verbose = self.timer_verbose)
self.timer_pose_est = TimerFps('PoseEst', is_verbose = self.timer_verbose)
self.timer_feat = TimerFps('Feature', is_verbose = self.timer_verbose)
# get current translation scale from ground-truth if grountruth is not None
def getAbsoluteScale(self, frame_id):
if self.grountruth is not None and kUseGroundTruthScale:
self.trueX, self.trueY, self.trueZ, scale = self.grountruth.getPoseAndAbsoluteScale(frame_id)
return scale
else:
self.trueX = 0
self.trueY = 0
self.trueZ = 0
return 1
def computeFundamentalMatrix(self, kps_ref, kps_cur):
F, mask = cv2.findFundamentalMat(kps_ref, kps_cur, cv2.FM_RANSAC, param1=kRansacThresholdPixels, param2=kRansacProb)
if F is None or F.shape == (1, 1):
# no fundamental matrix found
raise Exception('No fundamental matrix found')
elif F.shape[0] > 3:
# more than one matrix found, just pick the first
F = F[0:3, 0:3]
return np.matrix(F), mask
def removeOutliersByMask(self, mask):
if mask is not None:
n = self.kpn_cur.shape[0]
mask_index = [ i for i,v in enumerate(mask) if v > 0]
self.kpn_cur = self.kpn_cur[mask_index]
self.kpn_ref = self.kpn_ref[mask_index]
if self.des_cur is not None:
self.des_cur = self.des_cur[mask_index]
if self.des_ref is not None:
self.des_ref = self.des_ref[mask_index]
if kVerbose:
print('removed ', n-self.kpn_cur.shape[0],' outliers')
# fit essential matrix E with RANSAC such that: p2.T * E * p1 = 0 where E = [t21]x * R21
# out: [Rrc, trc] (with respect to 'ref' frame)
# N.B.1: trc is estimated up to scale (i.e. the algorithm always returns ||trc||=1, we need a scale in order to recover a translation which is coherent with previous estimated poses)
# N.B.2: this function has problems in the following cases: [see Hartley/Zisserman Book]
# - 'geometrical degenerate correspondences', e.g. all the observed features lie on a plane (the correct model for the correspondences is an homography) or lie on a ruled quadric
# - degenerate motions such a pure rotation (a sufficient parallax is required) or an infinitesimal viewpoint change (where the translation is almost zero)
# N.B.3: the five-point algorithm (used for estimating the Essential Matrix) seems to work well in the degenerate planar cases [Five-Point Motion Estimation Made Easy, Hartley]
# N.B.4: as reported above, in case of pure rotation, this algorithm will compute a useless fundamental matrix which cannot be decomposed to return the rotation
def estimatePose(self, kps_ref, kps_cur):
kp_ref_u = self.cam.undistort_points(kps_ref)
kp_cur_u = self.cam.undistort_points(kps_cur)
self.kpn_ref = self.cam.unproject_points(kp_ref_u)
self.kpn_cur = self.cam.unproject_points(kp_cur_u)
if kUseEssentialMatrixEstimation:
# the essential matrix algorithm is more robust since it uses the five-point algorithm solver by D. Nister (see the notes and paper above )
E, self.mask_match = cv2.findEssentialMat(self.kpn_cur, self.kpn_ref, focal=1, pp=(0., 0.), method=cv2.RANSAC, prob=kRansacProb, threshold=kRansacThresholdNormalized)
else:
# just for the hell of testing fundamental matrix fitting ;-)
F, self.mask_match = self.computeFundamentalMatrix(kp_cur_u, kp_ref_u)
E = self.cam.K.T @ F @ self.cam.K # E = K.T * F * K
#self.removeOutliersFromMask(self.mask) # do not remove outliers, the last unmatched/outlier features can be matched and recognized as inliers in subsequent frames
_, R, t, mask = cv2.recoverPose(E, self.kpn_cur, self.kpn_ref, focal=1, pp=(0., 0.))
return R,t # Rrc, trc (with respect to 'ref' frame)
def processFirstFrame(self):
# only detect on the current image
self.kps_ref, self.des_ref = self.feature_tracker.detectAndCompute(self.cur_image)
# convert from list of keypoints to an array of points
self.kps_ref = np.array([x.pt for x in self.kps_ref], dtype=np.float32)
self.draw_img = self.drawFeatureTracks(self.cur_image)
def processFrame(self, frame_id):
# track features
self.timer_feat.start()
self.track_result = self.feature_tracker.track(self.prev_image, self.cur_image, self.kps_ref, self.des_ref)
self.timer_feat.refresh()
# estimate pose
self.timer_pose_est.start()
R, t = self.estimatePose(self.track_result.kps_ref_matched, self.track_result.kps_cur_matched)
self.timer_pose_est.refresh()
# update keypoints history
self.kps_ref = self.track_result.kps_ref
self.kps_cur = self.track_result.kps_cur
self.des_cur = self.track_result.des_cur
self.num_matched_kps = self.kpn_ref.shape[0]
self.num_inliers = np.sum(self.mask_match)
if kVerbose:
print('# matched points: ', self.num_matched_kps, ', # inliers: ', self.num_inliers)
# t is estimated up to scale (i.e. the algorithm always returns ||trc||=1, we need a scale in order to recover a translation which is coherent with the previous estimated ones)
absolute_scale = self.getAbsoluteScale(frame_id)
if(absolute_scale > kAbsoluteScaleThreshold):
# compose absolute motion [Rwa,twa] with estimated relative motion [Rab,s*tab] (s is the scale extracted from the ground truth)
# [Rwb,twb] = [Rwa,twa]*[Rab,tab] = [Rwa*Rab|twa + Rwa*tab]
print('estimated t with norm |t|: ', np.linalg.norm(t), ' (just for sake of clarity)')
self.cur_t = self.cur_t + absolute_scale*self.cur_R.dot(t)
self.cur_R = self.cur_R.dot(R)
# draw image
self.draw_img = self.drawFeatureTracks(self.cur_image)
# check if we have enough features to track otherwise detect new ones and start tracking from them (used for LK tracker)
if (self.feature_tracker.tracker_type == FeatureTrackerTypes.LK) and (self.kps_ref.shape[0] < self.feature_tracker.num_features):
self.kps_cur, self.des_cur = self.feature_tracker.detectAndCompute(self.cur_image)
self.kps_cur = np.array([x.pt for x in self.kps_cur], dtype=np.float32) # convert from list of keypoints to an array of points
if kVerbose:
print('# new detected points: ', self.kps_cur.shape[0])
self.kps_ref = self.kps_cur
self.des_ref = self.des_cur
self.updateHistory()
def track(self, img, frame_id):
if kVerbose:
print('..................................')
print('frame: ', frame_id)
# convert image to gray if needed
if img.ndim>2:
img = cv2.cvtColor(img,cv2.COLOR_RGB2GRAY)
# check coherence of image size with camera settings
assert(img.ndim==2 and img.shape[0]==self.cam.height and img.shape[1]==self.cam.width), "Frame: provided image has not the same size as the camera model or image is not grayscale"
self.cur_image = img
# manage and check stage
if(self.stage == VoStage.GOT_FIRST_IMAGE):
self.processFrame(frame_id)
elif(self.stage == VoStage.NO_IMAGES_YET):
self.processFirstFrame()
self.stage = VoStage.GOT_FIRST_IMAGE
self.prev_image = self.cur_image
# update main timer (for profiling)
self.timer_main.refresh()
def drawFeatureTracks(self, img, reinit = False):
draw_img = cv2.cvtColor(img,cv2.COLOR_GRAY2RGB)
num_outliers = 0
if(self.stage == VoStage.GOT_FIRST_IMAGE):
if reinit:
for p1 in self.kps_cur:
a,b = p1.ravel()
cv2.circle(draw_img,(a,b),1, (0,255,0),-1)
else:
for i,pts in enumerate(zip(self.track_result.kps_ref_matched, self.track_result.kps_cur_matched)):
drawAll = False # set this to true if you want to draw outliers
if self.mask_match[i] or drawAll:
p1, p2 = pts
a,b = p1.ravel()
c,d = p2.ravel()
cv2.line(draw_img, (a,b),(c,d), (0,255,0), 1)
cv2.circle(draw_img,(a,b),1, (0,0,255),-1)
else:
num_outliers+=1
if kVerbose:
print('# outliers: ', num_outliers)
return draw_img
def updateHistory(self):
if (self.init_history is True) and (self.trueX is not None):
self.t0_est = np.array([self.cur_t[0], self.cur_t[1], self.cur_t[2]]) # starting translation
self.t0_gt = np.array([self.trueX, self.trueY, self.trueZ]) # starting translation
self.init_history = False
if (self.t0_est is not None) and (self.t0_gt is not None):
p = [self.cur_t[0]-self.t0_est[0], self.cur_t[1]-self.t0_est[1], self.cur_t[2]-self.t0_est[2]] # the estimated traj starts at 0
self.traj3d_est.append(p)
pg = [self.trueX-self.t0_gt[0], self.trueY-self.t0_gt[1], self.trueZ-self.t0_gt[2]] # the groudtruth traj starts at 0
self.traj3d_gt.append(pg)
self.poses.append(poseRt(self.cur_R, p))
from utils_img import combine_images_horizontally, rotate_img, transform_img, add_background
from utils_geom import add_ones
from utils_features import descriptor_sigma_mad, compute_hom_reprojection_error
from utils_draw import draw_feature_matches
from feature_tracker_configs import FeatureTrackerConfigs
from timer import TimerFps
# ==================================================================================================
# N.B.: test the feature tracker and its feature matching capability
# ==================================================================================================
timer = TimerFps(name='detection+description+matching')
#============================================
# Select Images
#============================================
img1, img2 = None, None # var initialization
img1_box = None # image 1 bounding box (initialization)
model_fitting_type = None # 'homography' or 'fundamental' (automatically set below, this is an initialization)
draw_horizontal_layout=True # draw matches with the two images in an horizontal or vertical layout (automatically set below, this is an initialization)
test_type='graf' # select the test type (there's a template below to add your test)
#
if test_type == 'box':
img1 = cv2.imread('../data/box.png') # queryImage
def __init__(self, camera, tracker, grountruth=None):
self.cam = camera
self.map = Map()
Frame.set_tracker(tracker) # set the static field of the class
# camera info
self.W, self.H = camera.width, camera.height
self.K = camera.K
self.Kinv = camera.Kinv
self.D = camera.D # distortion coefficients [k1, k2, p1, p2, k3]
self.stage = SLAMStage.NO_IMAGES_YET
self.intializer = Initializer()
self.cur_R = None # current rotation w.r.t. world frame
self.cur_t = None # current translation w.r.t. world frame
self.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of matched inliers
self.num_vo_map_points = None # current number of valid VO map points (matched and found valid in current pose optimization)
self.trueX, self.trueY, self.trueZ = None, None, None
self.grountruth = grountruth
self.mask_match = None
self.velocity = None
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.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_match = TimerFps('Match', is_verbose=self.timer_verbose)
self.timer_pose_est = TimerFps('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.timer_triangulation = TimerFps('Triangulation',
is_verbose=self.timer_verbose)
self.time_local_opt = TimerFps('Local optimization',
is_verbose=self.timer_verbose)
class SLAM(object):
def __init__(self, camera, tracker, grountruth=None):
self.cam = camera
self.map = Map()
Frame.set_tracker(tracker) # set the static field of the class
# camera info
self.W, self.H = camera.width, camera.height
self.K = camera.K
self.Kinv = camera.Kinv
self.D = camera.D # distortion coefficients [k1, k2, p1, p2, k3]
self.stage = SLAMStage.NO_IMAGES_YET
self.intializer = Initializer()
self.cur_R = None # current rotation w.r.t. world frame
self.cur_t = None # current translation w.r.t. world frame
self.num_matched_kps = None # current number of matched keypoints
self.num_inliers = None # current number of matched inliers
self.num_vo_map_points = None # current number of valid VO map points (matched and found valid in current pose optimization)
self.trueX, self.trueY, self.trueZ = None, None, None
self.grountruth = grountruth
self.mask_match = None
self.velocity = None
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.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_match = TimerFps('Match', is_verbose=self.timer_verbose)
self.timer_pose_est = TimerFps('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.timer_triangulation = TimerFps('Triangulation',
is_verbose=self.timer_verbose)
self.time_local_opt = TimerFps('Local optimization',
is_verbose=self.timer_verbose)
# fit essential matrix E with RANSAC such that: p2.T * E * p1 = 0 where E = [t21]x * R21
# out: [Rrc, trc] (with respect to 'ref' frame)
# N.B.1: trc is estimated up to scale (i.e. the algorithm always returns ||trc||=1, we need a scale in order to recover a translation which is coherent with previous estimated poses)
# N.B.2: this function has problems in the following cases: [see Hartley/Zisserman Book]
# - 'geometrical degenerate correspondences', e.g. all the observed features lie on a plane (the correct model for the correspondences is an homography) or lie a ruled quadric
# - degenerate motions such a pure rotation (a sufficient parallax is required) or an infinitesimal viewpoint change (where the translation is almost zero)
# N.B.3: the five-point algorithm (used for estimating the Essential Matrix) seems to work well in the degenerate planar cases [Five-Point Motion Estimation Made Easy, Hartley]
# N.B.4: as reported above, in case of pure rotation, this algorithm will compute a useless fundamental matrix which cannot be decomposed to return the rotation
def estimate_pose_ess_mat(self, kpn_ref, kpn_cur):
E, self.mask_match = cv2.findEssentialMat(
kpn_cur,
kpn_ref,
focal=1,
pp=(0., 0.),
method=cv2.RANSAC,
prob=kRansacProb,
threshold=kRansacThresholdNormalized)
_, R, t, mask = cv2.recoverPose(E,
kpn_cur,
kpn_ref,
focal=1,
pp=(0., 0.))
return poseRt(R, t.T) # Rrc,trc (cur with respect to 'ref' frame)
def track(self, img, frame_id, pose=None, verts=None):
# check image size is coherent with camera params
print("img.shape ", img.shape)
print("cam ", self.H, " x ", self.W)
assert img.shape[0:2] == (self.H, self.W)
self.timer_main_track.start()
# build current frame
self.timer_frame.start()
f_cur = Frame(self.map, img, self.K, self.Kinv, self.D, des=verts)
self.timer_frame.refresh()
if self.stage == SLAMStage.NO_IMAGES_YET:
# push first frame in the inizializer
self.intializer.init(f_cur)
self.stage = SLAMStage.NOT_INITIALIZED
return # EXIT (jump to second frame)
if self.stage == SLAMStage.NOT_INITIALIZED:
# try to inizialize
initializer_output, is_ok = self.intializer.initialize(f_cur, img)
if is_ok:
f_ref = self.intializer.f_ref
# add the two initialized frames in the map
self.map.add_frame(
f_ref) # add first frame in map and update its id
self.map.add_frame(
f_cur) # add second frame in map and update its id
# add points in map
new_pts_count, _ = self.map.add_points(
initializer_output.points4d, None, f_cur, f_ref,
initializer_output.idx_cur, initializer_output.idx_ref,
img)
Printer.green("map: initialized %d new points" %
(new_pts_count))
self.stage = SLAMStage.OK
return # EXIT (jump to next frame)
f_ref = self.map.frames[-1] # get previous frame in map
self.map.add_frame(f_cur) # add f_cur to map
# udpdate (velocity) motion model (kinematic without damping)
self.velocity = np.dot(f_ref.pose,
np.linalg.inv(self.map.frames[-2].pose))
predicted_pose = np.dot(self.velocity, f_ref.pose)
if kUseMotionModel is True:
print('using motion model')
# set intial guess for current pose optimization
f_cur.pose = predicted_pose.copy()
#f_cur.pose = f_ref.pose.copy() # get the last pose as an initial guess for optimization
if kUseSearchFrameByProjection:
# search frame by projection: match map points observed in f_ref with keypoints of f_cur
print('search frame by projection...')
idx_ref, idx_cur, num_found_map_pts = search_frame_by_projection(
f_ref, f_cur)
print("# found map points in prev frame: %d " %
num_found_map_pts)
else:
self.timer_match.start()
# find keypoint matches between f_cur and f_ref
idx_cur, idx_ref = match_frames(f_cur, f_ref)
self.num_matched_kps = idx_cur.shape[0]
print('# keypoint matches: ', self.num_matched_kps)
self.timer_match.refresh()
else:
print('estimating pose by fitting essential mat')
self.timer_match.start()
# find keypoint matches between f_cur and f_ref
idx_cur, idx_ref = match_frames(f_cur, f_ref)
self.num_matched_kps = idx_cur.shape[0]
print('# keypoint matches: ', self.num_matched_kps)
self.timer_match.refresh()
# N.B.: please, 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
Mrc = self.estimate_pose_ess_mat(f_ref.kpsn[idx_ref],
f_cur.kpsn[idx_cur])
Mcr = np.linalg.inv(poseRt(Mrc[:3, :3], Mrc[:3, 3]))
f_cur.pose = 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_index = (self.mask_match.ravel() == 1)
self.num_inliers = sum(mask_index)
print('# inliers: ', self.num_inliers)
idx_ref = idx_ref[mask_index]
idx_cur = idx_cur[mask_index]
# if too many outliers reset estimated pose
if self.num_inliers < kNumMinInliersEssentialMat:
f_cur.pose = f_ref.pose.copy(
) # reset estimated pose to previous frame
Printer.red('Essential mat: not enough inliers!')
# set intial guess for current pose optimization:
# keep the estimated rotation and override translation with ref frame translation (we do not have a proper scale for the translation)
f_cur.pose[:, 3] = f_ref.pose[:, 3].copy()
#f_cur.pose[:,3] = predicted_pose[:,3].copy() # or use motion model for translation
# populate f_cur with map points by propagating map point matches of f_ref:
# we use map points observed in f_ref and keypoint matches between f_ref and f_cur
num_found_map_pts_inter_frame = 0
if not kUseSearchFrameByProjection:
for i, idx in enumerate(idx_ref): # iterate over keypoint matches
if f_ref.points[
idx] is not None: # if we have a map point P for i-th matched keypoint in f_ref
f_ref.points[idx].add_observation(
f_cur, idx_cur[i]
) # then P is automatically matched to the i-th matched keypoint in f_cur
num_found_map_pts_inter_frame += 1
print("# matched map points in prev frame: %d " %
num_found_map_pts_inter_frame)
# f_cur pose optimization 1 (here we use first available information coming from first guess of f_cur pose and map points of f_ref matched keypoints )
self.timer_pose_opt.start()
pose_opt_error, pose_is_ok, self.num_vo_map_points = optimizer_g2o.poseOptimization(
f_cur, verbose=False)
print("pose opt err1: %f, ok: %d" % (pose_opt_error, int(pose_is_ok)))
# discard outliers detected in pose optimization (in current frame)
#f_cur.reset_outlier_map_points()
if pose_is_ok is True:
# discard outliers detected in f_cur pose optimization (in current frame)
f_cur.reset_outlier_map_points()
else:
# if current pose optimization failed, reset f_cur pose to f_ref pose
f_cur.pose = f_ref.pose.copy()
self.timer_pose_opt.pause()
# TODO: add recover in case of f_cur pose optimization failure
# 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 built local map) and {unmatched keypoints of f_cur}
if pose_is_ok is True and not self.map.local_map.is_empty():
self.timer_seach_map.start()
#num_found_map_pts = search_local_frames_by_projection(self.map, f_cur)
num_found_map_pts = search_map_by_projection(
self.map.local_map.points, f_cur) # use the built local map
print("# matched map points in local map: %d " % num_found_map_pts)
self.timer_seach_map.refresh()
# f_cur pose optimization 2 with all the matched map points
self.timer_pose_opt.resume()
pose_opt_error, pose_is_ok, self.num_vo_map_points = optimizer_g2o.poseOptimization(
f_cur, verbose=False)
print("pose opt err2: %f, ok: %d" %
(pose_opt_error, int(pose_is_ok)))
print("# valid matched map points: %d " % self.num_vo_map_points)
# discard outliers detected in pose optimization (in current frame)
if pose_is_ok is True:
f_cur.reset_outlier_map_points()
self.timer_pose_opt.refresh()
if kUseSearchFrameByProjection:
print("search for triangulation with epipolar lines...")
idx_ref, idx_cur, self.num_matched_kps, _ = search_frame_for_triangulation(
f_ref, f_cur, img)
print("# matched keypoints in prev frame: %d " %
self.num_matched_kps)
# if pose is ok, then we try to triangulate the matched keypoints (between f_cur and f_ref) that do not have a corresponding map point
if pose_is_ok is True and len(idx_ref) > 0:
self.timer_triangulation.start()
# TODO: this triangulation should be done from keyframes!
good_pts4d = np.array([
f_cur.points[i] is None for i in idx_cur
]) # matched keypoints of f_cur without a corresponding map point
# do triangulation in global frame
pts4d = triangulate_points(f_cur.pose, f_ref.pose,
f_cur.kpsn[idx_cur],
f_ref.kpsn[idx_ref], good_pts4d)
good_pts4d &= np.abs(pts4d[:, 3]) != 0
#pts4d /= pts4d[:, 3:]
pts4d[good_pts4d] /= pts4d[good_pts4d,
3:] # get homogeneous 3-D coords
new_pts_count, _ = self.map.add_points(pts4d,
good_pts4d,
f_cur,
f_ref,
idx_cur,
idx_ref,
img,
check_parallax=True)
print("# added map points: %d " % (new_pts_count))
self.timer_triangulation.refresh()
# local optimization
self.time_local_opt.start()
err = self.map.locally_optimize(local_window=kLocalWindow)
print("local optimization error: %f" % err)
self.time_local_opt.refresh()
# large window optimization of the map
# TODO: move this in a seperate thread with local mapping
if kUseLargeWindowBA is True and f_cur.id >= parameters.kEveryNumFramesLargeWindowBA and f_cur.id % parameters.kEveryNumFramesLargeWindowBA == 0:
err = self.map.optimize(
local_window=parameters.kLargeWindow) # verbose=True)
Printer.blue("large window optimization error: %f" % err)
print("map: %d points, %d frames" %
(len(self.map.points), len(self.map.frames)))
#self.updateHistory()
self.timer_main_track.refresh()
def updateHistory(self):
f_cur = self.map.frames[-1]
self.cur_R = f_cur.pose[:3, :3].T
self.cur_t = np.dot(-self.cur_R, f_cur.pose[:3, 3])
if (self.init_history is True) and (self.trueX is not None):
self.t0_est = np.array(
[self.cur_t[0], self.cur_t[1],
self.cur_t[2]]) # starting translation
self.t0_gt = np.array([self.trueX, self.trueY,
self.trueZ]) # starting translation
if (self.t0_est is not None) and (self.t0_gt is not None):
p = [
self.cur_t[0] - self.t0_est[0], self.cur_t[1] - self.t0_est[1],
self.cur_t[2] - self.t0_est[2]
] # the estimated traj starts at 0
self.traj3d_est.append(p)
self.traj3d_gt.append([
self.trueX - self.t0_gt[0], self.trueY - self.t0_gt[1],
self.trueZ - self.t0_gt[2]
])
self.poses.append(poseRt(self.cur_R, p))
# get current translation scale from ground-truth if this is set
def getAbsoluteScale(self, frame_id):
if self.grountruth is not None and kUseGroundTruthScale:
self.trueX, self.trueY, self.trueZ, scale = self.grountruth.getPoseAndAbsoluteScale(
frame_id)
return scale
else:
self.trueX = 0
self.trueY = 0
self.trueZ = 0
return 1
class LocalMapping(object):
def __init__(self, map):
self.map = map
self.recently_added_points = set()
self.kf_cur = None # current processed keyframe
self.kid_last_BA = -1 # last keyframe id when performed BA
self.descriptor_distance_sigma = Parameters.kMaxDescriptorDistance
self.timer_verbose = kTimerVerbose # set this to True if you want to print timings
self.timer_triangulation = TimerFps('Triangulation', is_verbose = self.timer_verbose)
self.timer_pts_culling = TimerFps('Culling points', is_verbose = self.timer_verbose)
self.timer_pts_fusion = TimerFps('Fusing points', is_verbose = self.timer_verbose)
self.time_local_opt = TimerFps('Local optimization', is_verbose = self.timer_verbose)
self.time_large_opt = TimerFps('Large window optimization', is_verbose = self.timer_verbose)
self.queue = Queue()
self.work_thread = Thread(target=self.run)
self.stop = False
self.lock_accept_keyframe = RLock()
self._is_idle = True
self.idle_codition = Condition()
self.opt_abort_flag = g2o.Flag(False)
self.log_file = None
self.thread_large_BA = None
def start(self):
self.work_thread.start()
def quit(self):
print('local mapping: quitting...')
self.stop = True
self.opt_abort_flag.value = True
self.work_thread.join()
print('local mapping: done')
def push_keyframe(self, keyframe):
self.queue.put(keyframe)
self.opt_abort_flag.value = True
def queue_size(self):
return self.queue.qsize()
def is_idle(self):
with self.lock_accept_keyframe:
return self._is_idle
def set_idle(self, flag):
with self.lock_accept_keyframe:
self._is_idle = flag
def wait_idle(self):
with self.idle_codition:
self.idle_codition.wait()
def interrupt_optimization(self):
Printer.yellow('interrupting local mapping optimization')
self.opt_abort_flag.value = True
def run(self):
while not self.stop:
with self.idle_codition:
self.set_idle(False)
self.do_local_mapping()
self.set_idle(True)
self.idle_codition.notifyAll()
time.sleep(kLocalMappingSleepTime)
def do_local_mapping(self):
if self.queue.empty():
return
if self.map.local_map.is_empty():
return
Printer.cyan('@local mapping')
time_start = time.time()
self.kf_cur = self.queue.get()
# if kLocalMappingOnSeparateThread:
# print('..................................')
# print('processing KF: ', self.kf_cur.id, ', queue size: ', self.queue_size())
#print('descriptor_distance_sigma: ', self.descriptor_distance_sigma)
self.process_new_keyframe()
# do map points culling
self.timer_pts_culling.start()
num_culled_points = self.cull_map_points()
self.timer_pts_culling.refresh()
print(' # culled points: ', num_culled_points)
# create new points by triangulation
self.timer_triangulation.start()
total_new_pts = self.create_new_map_points()
self.timer_triangulation.refresh()
print(" # new map points: %d " % (total_new_pts))
if self.queue.empty():
# fuse map points of close keyframes
self.timer_pts_fusion.start()
total_fused_pts = self.fuse_map_points()
self.timer_pts_fusion.refresh()
print(" # fused map points: %d " % (total_fused_pts))
# reset optimization flag
self.opt_abort_flag.value = False
if self.queue.empty():
if self.thread_large_BA is not None:
if self.thread_large_BA.is_alive(): # for security, check if large BA thread finished his work
self.thread_large_BA.join()
# launch local bundle adjustment
self.local_BA()
if kUseLargeWindowBA and \
self.kf_cur.kid >= Parameters.kEveryNumFramesLargeWindowBA and \
self.kid_last_BA != self.kf_cur.kid and self.kf_cur.kid % Parameters.kEveryNumFramesLargeWindowBA == 0:
# launch a parallel large window BA of the map
self.thread_large_BA = Thread(target=self.large_window_BA)
self.thread_large_BA.start()
# check redundant local Keyframes
num_culled_keyframes = self.cull_keyframes()
print(" # culled keyframes: %d " % (num_culled_keyframes))
duration = time.time() - time_start
print('local mapping duration: ', duration)
def local_BA(self):
# local optimization
self.time_local_opt.start()
err = self.map.locally_optimize(kf_ref=self.kf_cur, abort_flag=self.opt_abort_flag)
self.time_local_opt.refresh()
print("local optimization error^2: %f" % err)
num_kf_ref_tracked_points = self.kf_cur.num_tracked_points(kNumMinObsForKeyFrameDefault) # number of tracked points in k_ref
Printer.purple('KF(%d) #points: %d ' %(self.kf_cur.id, num_kf_ref_tracked_points))
def large_window_BA(self):
Printer.blue('@large BA')
# large window optimization of the map
self.kid_last_BA = self.kf_cur.kid
self.time_large_opt.start()
err = self.map.optimize(local_window=Parameters.kLargeBAWindow, abort_flag=self.opt_abort_flag) # verbose=True)
self.time_large_opt.refresh()
Printer.blue('large window optimization error^2: %f, KF id: %d' % (err,self.kf_cur.kid))
def process_new_keyframe(self):
# associate map points to keyframe observations (only good points)
# and update normal and descriptor
for idx,p in enumerate(self.kf_cur.get_points()):
if p is not None and not p.is_bad:
if p.add_observation(self.kf_cur, idx):
p.update_info()
else:
self.recently_added_points.add(p)
self.kf_cur.update_connections()
self.map.add_keyframe(self.kf_cur) # add kf_cur to map
def cull_map_points(self):
print('>>>> culling map points...')
th_num_observations = 2
min_found_ratio = 0.25
current_kid = self.kf_cur.kid
remove_set = set()
for p in self.recently_added_points:
if p.is_bad:
remove_set.add(p)
elif p.get_found_ratio() < min_found_ratio:
self.map.remove_point(p)
remove_set.add(p)
elif (current_kid - p.first_kid) >= 2 and p.num_observations <= th_num_observations:
self.map.remove_point(p)
remove_set.add(p)
elif (current_kid - p.first_kid) >= 3: # after three keyframes we do not consider the point a recent one
remove_set.add(p)
self.recently_added_points = self.recently_added_points - remove_set
num_culled_points = len(remove_set)
return num_culled_points
def cull_keyframes(self):
print('>>>> culling keyframes...')
num_culled_keyframes = 0
# check redundant keyframes in local keyframes: a keyframe is considered redundant if the 90% of the MapPoints it sees,
# are seen in at least other 3 keyframes (in the same or finer scale)
th_num_observations = 3
for kf in self.kf_cur.get_covisible_keyframes():
if kf.kid==0:
continue
kf_num_points = 0 # num good points for kf
kf_num_redundant_observations = 0 # num redundant observations for kf
for i,p in enumerate(kf.get_points()):
if p is not None and not p.is_bad:
kf_num_points += 1
if p.num_observations>th_num_observations:
scale_level = kf.octaves[i] # scale level of observation in kf
p_num_observations = 0
for kf_j,idx in p.observations():
if kf_j is kf:
continue
assert(not kf_j.is_bad)
scale_level_i = kf_j.octaves[idx] # scale level of observation in kfi
if scale_level_i <= scale_level+1: # N.B.1 <- more aggressive culling (expecially when scale_factor=2)
#if scale_level_i <= scale_level: # N.B.2 <- only same scale or finer
p_num_observations +=1
if p_num_observations >= th_num_observations:
break
if p_num_observations >= th_num_observations:
kf_num_redundant_observations += 1
if kf_num_redundant_observations > Parameters.kKeyframeCullingRedundantObsRatio * kf_num_points:
print('setting keyframe ', kf.id,' bad - redundant observations: ', kf_num_redundant_observations/max(kf_num_points,1),'%')
kf.set_bad()
num_culled_keyframes += 1
return num_culled_keyframes
def precompute_kps_matches(self, match_idxs, local_keyframes):
kf_pairs = []
if not Parameters.kLocalMappingParallelKpsMatching:
# do serial computation
for kf in local_keyframes:
if kf is self.kf_cur or kf.is_bad:
continue
idxs1, idxs2 = Frame.feature_matcher.match(self.kf_cur.des, kf.des)
match_idxs[(self.kf_cur,kf)]=(idxs1,idxs2)
else:
# do parallell computation
def thread_match_function(kf_pair):
kf1,kf2 = kf_pair
idxs1, idxs2 = Frame.feature_matcher.match(kf1.des, kf2.des)
match_idxs[(kf1, kf2)]=(idxs1,idxs2)
for kf in local_keyframes:
if kf is self.kf_cur or kf.is_bad:
continue
kf_pairs.append((self.kf_cur, kf))
with ThreadPoolExecutor(max_workers = Parameters.kLocalMappingParallelKpsMatchingNumWorkers) as executor:
executor.map(thread_match_function, kf_pairs) # automatic join() at the end of the `width` block
return match_idxs
# triangulate matched keypoints (without a corresponding map point) amongst recent keyframes
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
# fuse close map points of local keyframes
def fuse_map_points(self):
print('>>>> fusing map points')
total_fused_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, '...')
# search matches by projection from current KF in close KFs
for kf in local_keyframes:
if kf is self.kf_cur or kf.is_bad:
continue
num_fused_pts = search_and_fuse(self.kf_cur.get_points(), kf,
max_reproj_distance=Parameters.kMaxReprojectionDistanceFuse,
max_descriptor_distance=0.5*self.descriptor_distance_sigma) # finer search
print("# fused map points: %d for KFs (%d, %d)" % (num_fused_pts, self.kf_cur.id, kf.id))
total_fused_pts += num_fused_pts
# search matches by projection from local points in current KF
good_local_points = [p for kf in local_keyframes if not kf.is_bad for p in kf.get_points() if (p is not None and not p.is_bad) ] # all good points in local frames
good_local_points = np.array(list(set(good_local_points))) # be sure to get only one instance per point
num_fused_pts = search_and_fuse(good_local_points, self.kf_cur,
max_reproj_distance=Parameters.kMaxReprojectionDistanceFuse,
max_descriptor_distance=0.5*self.descriptor_distance_sigma) # finer search
print("# fused map points: %d for local map into KF %d" % (num_fused_pts, self.kf_cur.id))
total_fused_pts += num_fused_pts
# update points info
for p in self.kf_cur.get_points():
if p is not None and not p.is_bad:
p.update_info()
# update connections in covisibility graph
self.kf_cur.update_connections()
return total_fused_pts
from feature_manager import feature_manager_factory
from feature_types import FeatureDetectorTypes, FeatureDescriptorTypes, FeatureInfo
from utils_features import ssc_nms
from collections import defaultdict, Counter
from feature_manager_configs import FeatureManagerConfigs
from feature_tracker_configs import FeatureTrackerConfigs
from timer import TimerFps
# ==================================================================================================
# N.B.: here we test feature manager detectAndCompute()
# ==================================================================================================
timer = TimerFps()
#img = cv2.imread('../data/kitti06-12.png',cv2.IMREAD_COLOR)
#img = cv2.imread('../data/kitti06-435.png',cv2.IMREAD_COLOR)
img = cv2.imread('../data/kitti06-12-color.png', cv2.IMREAD_COLOR)
#img = cv2.imread('../data/mars1.png')
num_features = 2000
# select your tracker configuration (see the file feature_tracker_configs.py)
feature_tracker_config = FeatureTrackerConfigs.TEST
feature_tracker_config['num_features'] = num_features
feature_manager_config = FeatureManagerConfigs.extract_from(
feature_tracker_config)
print('feature_manager_config: ', feature_manager_config)