Added visualization of 3d trajectory error (gt vs aligned estimated).…

… Added some improvements for avoiding the first keyframe is without image.

feature_matcher.py

@@ -436,8 +436,12 @@ def match(self, img1, img2, des1, des2, kps1=None, kps2=None, ratio_test=None,
                     idxs1, idxs2 = MatcherUtils.rowMatchesWithRatioTest(matcher, kps1, des1, kps2, des2, max_descriptor_distance, max_disparity=max_disparity, ratio_test=ratio_test)
                 else:                             
                     idxs1, idxs2 = MatcherUtils.rowMatches(matcher, kps1, des1, kps2, des2, max_descriptor_distance, max_disparity=max_disparity)
-            result.idxs1 = idxs1
-            result.idxs2 = idxs2                               
+
+            # Change suggested here https://github.com/luigifreda/pyslam/issues/125#issuecomment-2555299806
+            #result.idxs1 = idxs1
+            #result.idxs2 = idxs2
+            result.idxs1 = idxs1.astype(np.int64)
+            result.idxs2 = idxs2.astype(np.int64)                              
             return result      
 
 

initializer.py

@@ -120,12 +120,14 @@ def check_num_failures(self):
 
     # push the first image
     def init(self, f_cur : Frame, img_cur):
+        f_cur.img = img_cur # enforce the image storage for the first frames: we need the keyframes to store images for loop detection
         self.frames.append(f_cur)    
         self.f_ref = f_cur
 
 
     # actually initialize having two available images                
     def initialize(self, f_cur: Frame, img_cur): 
+        f_cur.img = img_cur # enforce the image storage for the first frames: we need the keyframes to store images for loop detection
         return self.initialize_simple(f_cur, img_cur)     # try to inizialize current frame with reference frame: reference position is adjusted in the buffer
         #return self.initialize_enumerate(f_cur, img_cur) # try to inizialize current frame with all frames we can use in the buffer    
 
@@ -227,7 +229,7 @@ def process_frames(self, f_cur: Frame, img_cur, f_ref: Frame):
         #map.add_frame(f_ref)        
         #map.add_frame(f_cur)  
 
-        kf_ref = KeyFrame(f_ref)
+        kf_ref = KeyFrame(f_ref, f_ref.img)
         kf_cur = KeyFrame(f_cur, img_cur)        
         map.add_keyframe(kf_ref)        
         map.add_keyframe(kf_cur)      

loop_closing.py

@@ -787,7 +787,8 @@ def run(self):
                     print(f'LoopClosing: processing KF: {keyframe.id}, detection: qin size: {self.loop_detecting_process.q_in.qsize()}, qout size: {self.loop_detecting_process.q_out.qsize()}')       
 
                     # update the keyframe with the detection output
-                    keyframe.g_des = detection_output.g_des_vec             
+                    if keyframe.g_des is None:
+                        keyframe.g_des = detection_output.g_des_vec             
 
                     # for viz debugging
                     if self.store_kf_imgs:
@@ -809,7 +810,8 @@ def run(self):
                         if cov_kf_id in self.keyframes_map:
                             cov_kf = self.keyframes_map[cov_kf_id]
                             # update the cov keyframe with the detection output if needed
-                            if not cov_kf.is_bad and cov_kf.g_des is None:
+                            #if not cov_kf.is_bad and cov_kf.g_des is None:
+                            if cov_kf.g_des is None:
                                 cov_kf.g_des = detection_output.covisible_gdes_vecs[i]
 
                     got_loop = False

loop_detecting_process.py

@@ -41,11 +41,11 @@
 from keyframe import KeyFrame
 from frame import Frame
 
-from loop_detector_configs import LoopDetectorConfigs, loop_detector_factory, loop_detector_config_check, SlamFeatureManagerInfo
+from loop_detector_configs import LoopDetectorConfigs, loop_detector_factory, loop_detector_config_check, GlobalDescriptorType, SlamFeatureManagerInfo
 from loop_detector_base import LoopDetectorTask, LoopDetectorTaskType, LoopDetectorBase, LoopDetectorOutput
 
 import traceback
-
+import torch.multiprocessing as mp
 
 from typing import TYPE_CHECKING
 if TYPE_CHECKING:
@@ -76,6 +76,19 @@ class LoopDetectingProcess:
     def __init__(self, slam: 'Slam', loop_detector_config = LoopDetectorConfigs.DBOW3):
         set_rlimit()          
 
+        global_descriptor_type = loop_detector_config['global_descriptor_type']
+        # NOTE: the following set_start_method() is needed by multiprocessing for using CUDA acceleration (for instance with torch).
+        if global_descriptor_type ==  GlobalDescriptorType.COSPLACE or \
+           global_descriptor_type == GlobalDescriptorType.ALEXNET or \
+           global_descriptor_type == GlobalDescriptorType.NETVLAD or \
+           global_descriptor_type == GlobalDescriptorType.VLAD or \
+           global_descriptor_type == GlobalDescriptorType.EIGENPLACES:
+            if mp.get_start_method() != 'spawn':
+                mp.set_start_method('spawn', force=True) # NOTE: This may generate some pickling problems with multiprocessing 
+                                                        #       in combination with torch and we need to check it in other places.
+                                                        #       This set start method can be checked with MultiprocessingManager.is_start_method_spawn()
+
+
         self.loop_detector_config = loop_detector_config
         self.slam_info = SlamFeatureManagerInfo(slam=slam)
 

loop_detector_base.py

@@ -108,7 +108,7 @@ class LoopDetectorTask:
     def __init__(self, keyframe: KeyFrame, img, task_type=LoopDetectorTaskType.NONE, covisible_keyframes=[], connected_keyframes=[], load_save_path=None):
         self.task_type = task_type
         self.keyframe_data = LoopDetectKeyframeData(keyframe, img)
-        self.covisible_keyframes_data = [LoopDetectKeyframeData(kf) for kf in covisible_keyframes if not kf.is_bad] 
+        self.covisible_keyframes_data = [LoopDetectKeyframeData(kf) for kf in covisible_keyframes if not kf.is_bad and kf.id != self.keyframe_data.id] 
         self.connected_keyframes_ids =  [kf.id for kf in connected_keyframes]
         self.load_save_path = load_save_path            
         # # for loop closing 
@@ -262,7 +262,8 @@ def compute_reference_similarity_score(self, task: LoopDetectorTask, vector_type
                     #print(f'LoopDetectorBase: covisible keyframe {cov_kf.id} has no g_des, computing on img shape: {cov_kf.img.shape}, dtype: {cov_kf.img.dtype}')
                     if cov_kf.img.dtype != np.uint8:                    
                         print(f'LoopDetectorBase: covisible keyframe {cov_kf.id} has img dtype: {cov_kf.img.dtype}, converting to uint8')
-                        cov_kf.img = cov_kf.img.astype(np.uint8)            
+                        cov_kf.img = cov_kf.img.astype(np.uint8)
+                print(f'LoopDetectorBase: computing global descriptor for keyframe {cov_kf.id}')            
                 cov_kf.g_des = self.compute_global_des(cov_kf.des, cov_kf.img)
             if cov_kf.g_des is not None:             
                 if not isinstance(cov_kf.g_des, vector_type):

loop_detector_dbow2.py

@@ -148,6 +148,7 @@ def run_task(self, task: LoopDetectorTask):
 
         # compute global descriptor
         if keyframe.g_des is None:
+            print(f'LoopDetectorDBoW2: computing global descriptor for keyframe {keyframe.id}')            
             keyframe.g_des = self.compute_global_des(keyframe.des, keyframe.img) # get bow vector
             g_des_vec = keyframe.g_des.toVec() # transform it to vector(numpy array) to make it pickable
         else: 

loop_detector_dbow3.py

@@ -122,6 +122,7 @@ def run_task(self, task: LoopDetectorTask):
 
         # compute global descriptor
         if keyframe.g_des is None:
+            print(f'LoopDetectorDBoW3: computing global descriptor for keyframe {keyframe.id}')
             keyframe.g_des = self.compute_global_des(keyframe.des, keyframe.img) # get bow vector
             g_des_vec = keyframe.g_des.toVec() # transform it to a vector(numpy array) to make it pickable
         else: 

loop_detector_vlad.py

@@ -67,6 +67,14 @@
 class LoopDetectorVlad(LoopDetectorBase): 
     def __init__(self, vocabulary_data: VocabularyData, local_feature_manager=None):
         super().__init__()
+
+        import torch.multiprocessing as mp
+        # NOTE: the following set_start_method() is needed by multiprocessing for using CUDA acceleration (for instance with torch).        
+        if mp.get_start_method() != 'spawn':
+            mp.set_start_method('spawn', force=True) # NOTE: This may generate some pickling problems with multiprocessing 
+                                                    #       in combination with torch and we need to check it in other places.
+                                                    #       This set start method can be checked with MultiprocessingManager.is_start_method_spawn()
+
         self.local_feature_manager = local_feature_manager        
         self.use_torch_vectors = False # use torch vectors with a simple database implementation
 
@@ -145,6 +153,7 @@ def run_task(self, task: LoopDetectorTask):
 
         # compute global descriptor
         if keyframe.g_des is None:
+            print(f'LoopDetectorVlad: computing global descriptor for keyframe {keyframe.id}')
             keyframe.g_des = self.compute_global_des(keyframe.des, keyframe.img) # get global descriptor
 
         #print(f'LoopDetectorVlad: g_des = {keyframe.g_des}, type: {type(keyframe.g_des)}, shape: {keyframe.g_des.shape}, dim: {keyframe.g_des.dim()}')

loop_detector_vpr.py

@@ -111,7 +111,7 @@ def __init__(self, global_descriptor_name= None, local_feature_manager=None, nam
             raise ValueError('LoopDetectorVprBase: global_descriptor_name cannot be None')
 
         self.score = None           
-        if self.global_descriptor_name == 'SAD':
+        if self.global_descriptor_name.lower() == 'sad':
             self.score = ScoreSad()
             self.min_score = -100
             if Parameters.kLoopClosingDebugWithSimmetryMatrix:
@@ -127,15 +127,16 @@ def __init__(self, global_descriptor_name= None, local_feature_manager=None, nam
         self.global_feature_extractor = None
         self.global_db = None
 
-        # NOTE: the following set_start_method() is needed by multiprocessing for using CUDA acceleration (with torch).
-        if global_descriptor_name == 'CosPlace' or \
-            global_descriptor_name == 'AlexNet' or \
-            global_descriptor_name == 'NetVLAD' or \
-            global_descriptor_name == 'EigenPlaces':
+        # NOTE: The following set_start_method() is needed by multiprocessing for using CUDA acceleration (for instance with torch).
+        if global_descriptor_name.lower() == 'cosplace' or \
+            global_descriptor_name.lower() == 'alexnet' or \
+            global_descriptor_name.lower() == 'netvlad' or \
+            global_descriptor_name.lower() == 'eigenplaces':
             import torch.multiprocessing as mp
-            mp.set_start_method('spawn', force=True) # NOTE: This generates some pickling problems with multiprocessing 
-                                                     #       in combination with torch and we need to check it in other places.
-                                                     #       This set start method can be checked with MultiprocessingManager.is_start_method_spawn()
+            if mp.get_start_method() != 'spawn':
+                mp.set_start_method('spawn', force=True) # NOTE: This may generate some pickling problems with multiprocessing 
+                                                         #       in combination with torch and we need to check it in other places.
+                                                         #       This set start method can be checked with MultiprocessingManager.is_start_method_spawn()
 
         #self.init() # NOTE: We call init() in the run_task() method at its first call to 
                      #       initialize the global feature extractor in the potentially launched parallel process.
@@ -185,16 +186,16 @@ def init_db(self):
     def init_global_feature_extractor(self, global_descriptor_name):
         print(f'LoopDetectorVprBase: init_global_feature_extractor: global_descriptor_name: {global_descriptor_name}')
         global_feature_extractor = None
-        if global_descriptor_name == 'HDC-DELF':
+        if global_descriptor_name.lower() == 'hdc-delf':
             from feature_extraction.feature_extractor_holistic import HDCDELF
             global_feature_extractor = HDCDELF()
-        elif global_descriptor_name == 'AlexNet':
+        elif global_descriptor_name.lower() == 'alexnet':
             from feature_extraction.feature_extractor_holistic import AlexNetConv3Extractor
             global_feature_extractor = AlexNetConv3Extractor()
-        elif global_descriptor_name == 'SAD':
+        elif global_descriptor_name.lower() == 'sad':
             from feature_extraction.feature_extractor_holistic import SAD
             global_feature_extractor = SAD()
-        elif global_descriptor_name == 'NetVLAD':
+        elif global_descriptor_name.lower() == 'netvlad':
             from feature_extraction.feature_extractor_patchnetvlad import PatchNetVLADFeatureExtractor
             from patchnetvlad.tools import PATCHNETVLAD_ROOT_DIR
             import configparser
@@ -204,10 +205,10 @@ def init_global_feature_extractor(self, global_descriptor_name):
             config = configparser.ConfigParser()
             config.read(configfile)
             global_feature_extractor = PatchNetVLADFeatureExtractor(config)
-        elif global_descriptor_name == 'CosPlace':
+        elif global_descriptor_name.lower() == 'cosplace':
             from feature_extraction.feature_extractor_cosplace import CosPlaceFeatureExtractor
             global_feature_extractor = CosPlaceFeatureExtractor()
-        elif global_descriptor_name == 'EigenPlaces':
+        elif global_descriptor_name.lower() == 'eigenplaces':
             from feature_extraction.feature_extractor_eigenplaces import EigenPlacesFeatureExtractor
             global_feature_extractor = EigenPlacesFeatureExtractor()
         else:
@@ -242,6 +243,7 @@ def run_task(self, task: LoopDetectorTask):
 
         # compute global descriptor
         if keyframe.g_des is None:
+            print(f'LoopDetectorVprBase: computing global descriptor for keyframe {keyframe.id}')
             keyframe.g_des = self.compute_global_des(keyframe.des, keyframe.img) # get global descriptor
 
         if task.task_type != LoopDetectorTaskType.RELOCALIZATION:

mplot_thread.py

@@ -116,7 +116,8 @@ def __init__(self, xlabel='', ylabel='', title=''):
 
         self.data = None 
         self.got_data = False 
-        self.handle_map = {}  
+        self.handle_map = {}
+        self.fig = None  
 
         self.axis_computed = False 
         self.xlim = [float("inf"),float("-inf")]