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)
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)
