slam.py

"""
* This file is part of PYSLAM 
*
* Copyright (C) 2016-present Luigi Freda <luigi dot freda at gmail dot com> 
*
* PYSLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* PYSLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with PYSLAM. If not, see <http://www.gnu.org/licenses/>.
"""

import numpy as np
import time
from enum import Enum

from collections import defaultdict, Counter
from itertools import chain

import cv2
import g2o

from parameters import Parameters  

from frame import Frame, match_frames
from keyframe import KeyFrame
from map_point import MapPoint
from map import Map

from search_points import propagate_map_point_matches
from search_points import search_map_by_projection, search_frame_by_projection

from local_mapping import LocalMapping
from initializer import Initializer
import optimizer_g2o

from timer import TimerFps

from slam_dynamic_config import SLAMDynamicConfig
from motion_model import MotionModel, MotionModelDamping

from feature_tracker import FeatureTrackerTypes 

from utils_sys import Printer, getchar, Logging
from utils_draw import draw_feature_matches
from utils_geom import triangulate_points, poseRt, normalize_vector, inv_T, triangulate_normalized_points, estimate_pose_ess_mat


kVerbose = True     
kTimerVerbose = False 
kDebugDrawMatches = False 

kLocalMappingOnSeparateThread = Parameters.kLocalMappingOnSeparateThread 
kTrackingWaitForLocalMappingToGetIdle = Parameters.kTrackingWaitForLocalMappingToGetIdle
kTrackingWaitForLocalMappingSleepTime = Parameters.kTrackingWaitForLocalMappingSleepTime

kLogKFinfoToFile = True 

kUseDynamicDesDistanceTh = True  

kRansacThresholdNormalized = 0.0003  # 0.0003 # metric threshold used for normalized image coordinates 
kRansacProb = 0.999
kNumMinInliersEssentialMat = 8

kUseGroundTruthScale = False 

kNumMinInliersPoseOptimizationTrackFrame = 10
kNumMinInliersPoseOptimizationTrackLocalMap = 20

kUseMotionModel = Parameters.kUseMotionModel or Parameters.kUseSearchFrameByProjection
kUseSearchFrameByProjection = Parameters.kUseSearchFrameByProjection and not Parameters.kUseEssentialMatrixFitting         
kUseEssentialMatrixFitting = Parameters.kUseEssentialMatrixFitting      
       
kNumMinObsForKeyFrameDefault = 3


if not kVerbose:
    def print(*args, **kwargs):
        pass 


class SlamState(Enum):
    NO_IMAGES_YET=0,
    NOT_INITIALIZED=1,
    OK=2,
    LOST=3


class TrackingHistory(object):
    def __init__(self):
        self.relative_frame_poses = []  # list of relative frame poses as g2o.Isometry3d() (see camera_pose.py)
        self.kf_references = []         # list of reference keyframes  
        self.timestamps = []            # list of frame timestamps 
        self.slam_states = []           # list of slam states 


# main slam class containing all the required modules 
class Slam(object):
    def __init__(self, camera, feature_tracker, groundtruth = None):    
        self.init_feature_tracker(feature_tracker)
        self.camera = camera
        self.map = Map()
        self.local_mapping = LocalMapping(self.map)
        if kLocalMappingOnSeparateThread:
            self.local_mapping.start()
        self.groundtruth = groundtruth  # not actually used here; could be used for evaluating performances 
        self.tracking = Tracking(self)

        
    def quit(self):
        if kLocalMappingOnSeparateThread:
            self.local_mapping.quit()                       


    def init_feature_tracker(self, tracker):
        Frame.set_tracker(tracker) # set the static field of the class 
        if kUseEssentialMatrixFitting:
            Printer.orange('forcing feature matcher ratio_test to 0.8')
            tracker.matcher.ratio_test = 0.8
        if tracker.tracker_type == FeatureTrackerTypes.LK:
            raise ValueError("You cannot use Lukas-Kanade tracker in this SLAM approach!")  
        
        
    # @ main track method @
    def track(self, img, frame_id, timestamp=None, segmentation=None):
        return self.tracking.track(img,frame_id,timestamp, segmentation=segmentation)


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, segmentation=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, segmentation=segmentation) 
        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, segmentation)
            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, segmentation=segmentation)
                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, segmentation=segmentation)                                     
                    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)  
        # check again for debug                     
        #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)             


    # def update_history(self):
    #     f_cur = self.map.get_frame(-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 get_absolute_scale(self, frame_id):  
    #     if self.groundtruth is not None and kUseGroundTruthScale:
    #         self.trueX, self.trueY, self.trueZ, scale = self.groundtruth.getPoseAndAbsoluteScale(frame_id)
    #         return scale
    #     else:
    #         self.trueX = 0 
    #         self.trueY = 0 
    #         self.trueZ = 0
    #         return 1