added

- tests
- small improvements on algorithms
- updates on interfaces

config.ini

@@ -2,25 +2,33 @@
 
 ; use ';' for commenting a line!
 
+
 [LIB_PATH]
 g2o=lib/g2opy/lib
 pangolin=lib/pangolin
 
+
 [DATASET]
 ; 1 step select your dataset 
 ;type=KITTI_DATASET
 ;type=TUM_DATASET
 type=VIDEO_DATASET
 ;type=FOLDER_DATASET
 
+
 [KITTI_DATASET]
 type=kitti 
 base_path=/home/luigi/Work/rgbd_datasets/kitti/dataset
-name=06
-;cam_settings=settings/KITTI00-02.yaml
-cam_settings=settings/KITTI04-12.yaml
+;
+name=00
+cam_settings=settings/KITTI00-02.yaml
+;
+#name=06
+#cam_settings=settings/KITTI04-12.yaml
+;
 groundtruth_file=auto
 
+
 [TUM_DATASET]
 type=tum
 base_path=/home/luigi/Work/rgbd_datasets
@@ -29,15 +37,20 @@ cam_settings=settings/TUM1.yaml
 associations=associations.txt
 groundtruth_file=auto
 
+
 [VIDEO_DATASET]
 type=video
-;base_path=./videos/kitti00
-;cam_settings=settings/KITTI00-02.yaml
-base_path=./videos/kitti06
-cam_settings=settings/KITTI04-12.yaml
+;
+base_path=./videos/kitti00
+cam_settings=settings/KITTI00-02.yaml
+;
+#base_path=./videos/kitti06
+#cam_settings=settings/KITTI04-12.yaml
+;
 name=video.mp4
 groundtruth_file=groundtruth.txt
 
+
 [FOLDER_DATASET]
 type=folder 
 base_path=/home/luigi/Work/rgbd_datasets/kitti/dataset/sequences/00/image_0/

dataset.py

@@ -24,6 +24,8 @@
 import os
 import glob
 
+from multiprocessing import Process, Queue
+
 
 class DatasetType(Enum):
     NONE = 1
@@ -154,6 +156,88 @@ def getImage(self, frame_id):
         self.i = self.i + 1
         return img
 
+class FolderDatasetParallelStatus:
+    def __init__(self, i, maxlen, listing, skip):
+        self.i = i
+        self.maxlen = maxlen
+        self.listing = listing 
+        self.skip = skip  
+
+class FolderDatasetParallel(Dataset): 
+    def __init__(self, path, name, associations=None, type=DatasetType.VIDEO): 
+        super().__init__(path, name, associations, type)    
+        self.skip=1
+        self.listing = []    
+        self.maxlen = 1000000    
+        print('Processing Image Directory Input')
+        self.listing = glob.glob(path + '/' + self.name)
+        self.listing.sort()
+        self.listing = self.listing[::self.skip]
+        #print('list of files: ', self.listing)
+        self.maxlen = len(self.listing)
+        self.i = 0        
+        if self.maxlen == 0:
+          raise IOError('No images were found in folder: ', path)     
+
+        folder_status = FolderDatasetParallelStatus(i,maxlen,listing,skip)
+
+        self.q = Queue(maxsize=2)        
+        self.vp = Process(target=self._update_image, args=(self.q,folder_status,))
+        self.vp.daemon = True                 
+
+    def _update_image(self, q, status):
+        while(True):
+            self.ret, self.current_frame = self.cap.read()
+            if self.ret is True: 
+                #self.current_frame= self.cap.read()[1]
+                #if q.full():
+                #    old_frame = self.q.get()
+                self.q.put(self.current_frame)
+                print('q.size: ', self.q.qsize())
+        time.sleep(0.005)
+
+    def getImage(self, frame_id):
+        if self.i == self.maxlen:
+            return (None, False)
+        image_file = self.listing[self.i]
+        img = cv2.imread(image_file, 0)
+        if img is None: 
+            raise IOError('error reading file: ', image_file)               
+        # Increment internal counter.
+        self.i = self.i + 1
+        return img 
+
+class Webcam(object):
+    def __init__(self, camera_num=0):
+        self.cap = cv2.VideoCapture(camera_num)
+        self.current_frame = None 
+        self.ret = None 
+        self.q = Queue(maxsize=2)        
+        self.vp = Process(target=self._update_frame, args=(self.q,))
+        self.vp.daemon = True
+
+    # create thread for capturing images
+    def start(self):
+        self.vp.start()        
+
+    # process function     
+    def _update_frame(self, q):
+        while(True):
+            self.ret, self.current_frame = self.cap.read()
+            if self.ret is True: 
+                #self.current_frame= self.cap.read()[1]
+                #if q.full():
+                #    old_frame = self.q.get()
+                self.q.put(self.current_frame)
+                print('q.size: ', self.q.qsize())
+        time.sleep(0.005)
+
+    # get the current frame
+    def get_current_frame(self):
+        img = None 
+        while not self.q.empty():  # get the last one 
+            img = self.q.get()         
+        return img
 
 class KittiDataset(Dataset):
     def __init__(self, path, name, associations=None, type=DatasetType.KITTI): 

feature_detector.py

@@ -266,10 +266,10 @@ def __init__(self, min_num_features=kMinNumFeatureDefault,
             self._feature_descriptor = ORB_create(**self.orb_params) 
             self.decriptor_name = 'ORB' 
         elif self.descriptor_type == FeatureDescriptorTypes.BRISK:
-            self._feature_descriptor = BRISK_create() 
+            self._feature_descriptor = BRISK_create(octaves=self.num_levels) 
             self.decriptor_name = 'BRISK'         
         elif self.descriptor_type == FeatureDescriptorTypes.AKAZE:
-            self._feature_descriptor = AKAZE_create() 
+            self._feature_descriptor = AKAZE_create(nOctaves=self.num_levels) 
             self.decriptor_name = 'AKAZE'                                                
         elif self.descriptor_type == FeatureDescriptorTypes.NONE:
             self._feature_descriptor = None              
@@ -329,7 +329,7 @@ def detectAndCompute(self, frame, mask=None):
     # keep the first 'self.min_num_features' best features
     def satNumberOfFeatures(self, kps):
         if kVerbose:
-            print('detector: ', self.detector_name, ', #features: ', len(kps),', ref: ', self.min_num_features)          
+            print('sat: ', self.detector_name, ', #features: ', len(kps),', #max: ', self.min_num_features)          
         if len(kps) > self.min_num_features:
             # keep the features with the best response 
             kps = sorted(kps, key=lambda x:x.response, reverse=True)[:self.min_num_features]         

feature_matcher.py

@@ -63,20 +63,26 @@ def goodMatches(self, matches, des1, des2):
                 elif len(lm)==1:
                     m = lm[0]
                 else:
+                    # we have two mathes for queryIdx 
                     if lm[0].distance < kRatioTest * lm[1].distance:
                         m = lm[0]
+                        #print("match1: %d, %d" % (lm[0].queryIdx,lm[0].trainIdx))
+                        #print("match2: %d, %d" % (lm[1].queryIdx,lm[1].trainIdx))                        
                     else:
                         continue     
-                if not flag_match[m.trainIdx]:
+                if not flag_match[m.trainIdx]: 
+                    # trainIdx has not been matched yet
                     flag_match[m.trainIdx] = True
                     dist_match[m.trainIdx] = m.distance
                     #good_matches.append(m)
                     idx1.append(m.queryIdx)
                     idx2.append(m.trainIdx)
                     index_match[m.trainIdx] = len(idx2)-1
                 else:
-                    # store match is worse => replace it
+                    # we have already a match for trainIdx 
+                    # if stored match is worse => replace it
                     if dist_match[m.trainIdx] > m.distance:
+                        #print("double match on trainIdx: ", m.trainIdx)
                         index = index_match[m.trainIdx]
                         assert(idx2[index] == m.trainIdx) 
                         #good_matches[index] = m 

feature_tracker.py

@@ -22,6 +22,7 @@
 from enum import Enum
 from feature_detector import feature_detector_factory, FeatureDetectorTypes, FeatureDescriptorTypes
 from feature_matcher import feature_matcher_factory, FeatureMatcherTypes
+from helpers import Printer
 
 
 class TrackerTypes(Enum):
@@ -85,7 +86,10 @@ def __init__(self, min_num_features=kMinNumFeatureDefault,
                        num_levels = 3,
                        detector_type = FeatureDetectorTypes.FAST, 
                        descriptor_type = FeatureDescriptorTypes.NONE, 
-                       tracker_type = TrackerTypes.LK):    
+                       tracker_type = TrackerTypes.LK):   
+        if num_levels < 3:
+            Printer.green('LkFeatureTracker: forcing at least 3 levels') 
+            num_levels = 3                        
         super().__init__(min_num_features, num_levels, detector_type, descriptor_type, tracker_type)
         self.detector = feature_detector_factory(min_num_features, num_levels, detector_type, descriptor_type)   
         # we use LK pyr optic flow for matching     

frame.py

@@ -28,7 +28,7 @@
 from feature_detector import feature_detector_factory, FeatureDetectorTypes, FeatureDescriptorTypes
 from feature_tracker import feature_tracker_factory, TrackerTypes 
 
-import constants 
+import parameters  
 
 kDrawFeatureRadius = [r*3 for r in range(1,20)]
 
@@ -62,6 +62,8 @@ def __init__(self, mapp, img, K, Kinv, DistCoef, pose=np.eye(4), tid=None, des=N
         self.D = np.array(DistCoef)
 
         self.pose = np.array(pose)  # Tcw
+        self.Ow = np.zeros((3,1))
+        self.Rwc = np.eye(3)
 
         # self.kps       keypoints
         # self.kpsu      [u]ndistorted keypoints
@@ -88,7 +90,7 @@ def __init__(self, mapp, img, K, Kinv, DistCoef, pose=np.eye(4), tid=None, des=N
                 self.kpsu = self.kpsu.ravel().reshape(self.kps.shape[0], 2)
                 #print('kpsu diff: ', self.kpsu-self.kps)
             else:
-                assert len(des) < 256
+                #assert len(des) < 256
                 self.kpsu, self.des = des, np.array(list(range(len(des)))*32, np.uint8).reshape(32, len(des)).T
                 self.octaves = np.full(self.kpsu.shape[0], 1, dtype=np.uint8)
             self.kpsn = normalize(self.Kinv, self.kpsu)
@@ -101,21 +103,39 @@ def __init__(self, mapp, img, K, Kinv, DistCoef, pose=np.eye(4), tid=None, des=N
         self.id = Frame.next_id #tid if tid is not None else mapp.add_frame(self)
         Frame.next_id+=1
 
+    def update_camera_pose(self):
+        self.Rcw = self.pose[:3,:3]
+        self.tcw = self.pose[:3,3].reshape(3,1)
+        self.Ow = -(self.Rcw.T @ self.tcw)
+
+    # project a list of N points4d on this frame: points is [Nx4]
+    # out: 2xN image points
+    def project_points(self, points4d):   
+        projs = np.dot(np.dot(self.K, self.pose[:3,:]), points4d.T).T
+        projs = projs[:, 0:2] / projs[:, 2:]    
+        return projs   
+
     # project a list of N map points on this frame
     # out: 2xN image points 
     def project_map_points(self, points):
         points4d = np.array([p.homogeneous() for p in points])
-        projs = np.dot(np.dot(self.K, self.pose[:3,:]), points4d.T).T
-        projs = projs[:, 0:2] / projs[:, 2:]    
-        return projs
+        return self.project_points(points4d)
 
     # project a single map point on this frame
     # out: 2x1 image points 
     def project_map_point(self, p):   
-        proj = np.dot(np.dot(self.K, self.pose[:3,:]), p.homogeneous().T).T
+        proj = np.dot(np.dot(self.K, self.pose[:3,:]), p.homogeneous().T).T   # K * [R , t] * p
         proj = proj[0:2]/ proj[2]    
         return proj    
 
+    # project a single world 3d point on this frame
+    def project_point(self, pw):   
+        pc = (self.Rcw @ pw) + self.tcw # p w.r.t. camera 
+        invz = 1./pc[2]
+        proju = self.fx*pc[0]*invz + self.cx  
+        projv = self.fy*pc[1]*invz + self.cy 
+        return np.array([proju, projv])     
+
     def reset_outlier_map_points(self):
         for i,p in enumerate(self.points):
             if p is not None and self.outliers[i] is True: 
@@ -141,6 +161,14 @@ def reset_points(self):
         self.points = [None]*len(self.kpsu) 
         self.outliers = np.full(self.kpsu.shape[0], False, dtype=bool)                 
 
+    def compute_points_median_depth(self, points4d):
+        Rcw2 = self.pose[2,:3]  # just 2-nd row 
+        tcw2 = self.pose[2,3]   # just 2-nd row       
+        z = np.dot(Rcw2, points4d[:,:3].T) + tcw2 
+        z = sorted(z) 
+        return z[ ( len(z)-1)//2 ]
+
+
     # KD tree of unnormalized keypoints
     @property
     def kd(self):