tracking features from TUM VI and MP3D-VO ready

.env

@@ -1,11 +1,5 @@
-
-MAIN_DIR=${PWD}
-UTILS_DIR=${MAIN_DIR}/utils
-DATA_DIR=${MAIN_DIR}/data
-CONF_DIR=${MAIN_DIR}/config
-CFG_DEFAULT_FILE=${CONF_DIR}/config.yaml
-
+DIR_ROOT=${PWD}
 
 # ! Dataset directories
-MP3D_VO_DATASET_DIR="/home/kike/Documents/datasets/MP3D_VO"
-TUM_VI_DATASET_DIR="/home/kike/Documents/datasets/MP3D_VO"
+DIR_MP3D_VO_DATASET=${HOME}/Documents/datasets/MP3D_VO
+DIR_TUM_VI_DATASET=${HOME}/Documents/datasets/TUM_VI

.gitignore

@@ -9,3 +9,5 @@ analysis/performance_comparisons/plots
 report
 report_backup
 *.mp4
+
+*.code-workspace

config/config_MP3D_VO.yaml

@@ -0,0 +1,26 @@
+# Configurations 
+
+# Dataset settings
+dataset: MP3D_VO
+scene: 2n8kARJN3HM
+scene_version: 0
+
+# Data settings
+initial_frame: 0              #* intial frame to read automatically
+special_tracking: False       #* track every frame as inital ref for tracking
+min_cam_distance: 0.5         #* min camera motion between ref and tracked frame
+show_tracked_features: True   #* show while tracking
+save_bearings: True           #* save bearing vectors which are evaluated
+max_number_features: 200      #* Number of features (sampling from all tracked features)
+
+# Shi-Tomasi feature extractor
+max_number_corners: 500       #* Number of extracted feature by the extarctor
+quality_corner_level: 0.0001  #* Quality of corners (higher better but less features)
+min_corner_distance: 10       #* Min distance bewteen extracted features
+block_size_for_corners: 10    #* Windows patch used to compute a feature
+
+# LK tracker
+coarse_fine_levels: 2         #* Number of course-fine level used to track features
+block_size_for_tracking: 10
+eps_tracking: 0.0001
+counter_iterations: 5

config/config_TUM_VI.yaml

@@ -0,0 +1,26 @@
+# Configurations 
+
+# Dataset settings
+dataset: TUM_VI
+scene: 1
+scene_version: "512_16"
+
+# Data settings
+initial_frame: 0              #* intial frame to read automatically
+special_tracking: False       #* track every frame as inital ref for tracking
+min_cam_distance: 0.5         #* min camera motion between ref and tracked frame
+show_tracked_features: True   #* show while tracking
+save_bearings: True           #* save bearing vectors which are evaluated
+max_number_features: 200      #* Number of features (sampling from all tracked features)
+
+# Shi-Tomasi feature extractor
+max_number_corners: 1000       #* Number of extracted feature by the extarctor
+quality_corner_level: 0.01  #* Quality of corners (higher better but less features)
+min_corner_distance: 10       #* Min distance bewteen extracted features
+block_size_for_corners: 5    #* Windows patch used to compute a feature
+
+# LK tracker
+coarse_fine_levels: 8         #* Number of course-fine level used to track features
+block_size_for_tracking: 15
+eps_tracking: 0.1
+counter_iterations: 10

dataset_reader/MP3D_VO.py

@@ -1,16 +1,16 @@
+import sys
+sys.path.append("/home/kike/Documents/Research/Utilities")
+sys.path.append("/home/kike/Documents/Research/CameraModels")
+from pcl_utilities import *
+import numpy as np
+from projections.equi2pcl.equi2pcl import Equi2PCL
+from .dataset import Data
+from TUM_RGBD_utils.evaluate_rpe import read_trajectory
 from config import Cfg
 import os
 from imageio import imread
 import matplotlib.pyplot as plt
-import sys
-sys.path.append("/home/kike/Documents/Research/Utilities")
-sys.path.append("/home/kike/Documents/Research/CameraModels")
 
-from TUM_RGBD_utils.evaluate_rpe import read_trajectory
-from .dataset import Data
-from projections.equi2pcl.equi2pcl import Equi2PCL
-import numpy as np
-from pcl_utilities import *
 
 
 class MP3D_VO(Data):
@@ -23,12 +23,12 @@ def __init__(self, dt_dir, scene):
         self.camera_poses = read_trajectory(self.scene_dir + "/frm_ref.txt")
         # self.camera_poses = pd.read_csv(self.dir_path + "/frm_ref.txt", header=None, sep=" ",).values
         self.number_frames = len(self.camera_poses)
-        self.time_stamps = tuple(self.camera_poses.keys())
+        self.timestamps = tuple(self.camera_poses.keys())
         if not self.number_frames == len(os.listdir(self.dir_rgb)):
             print(
                 "Warn: Number of frames in frm_ref.txt do not match with rgb files"
             )
-        color_map, depth_map, _, _ = self.get_frame(0)
+        color_map, depth_map, _, _, _ = self.get_frame(0)
         self.shape = (color_map.shape[0], color_map.shape[1])
         self.internal_index = 0
         # ! this allows us to project a equirectangular projection into pcl
@@ -42,7 +42,7 @@ def get_rgb(self, idx):
             rgb = imread(os.path.join(self.dir_rgb, "{}.jpg".format(idx + 1)))
 
         return rgb
-        
+
     def get_frame(self, idx, return_dict=False, **kwargs):
         assert idx < self.number_frames
         try:
@@ -58,53 +58,56 @@ def get_frame(self, idx, return_dict=False, **kwargs):
             depth = None
             # print("No depth information !!!!")
 
-        pose = self.camera_poses[self.time_stamps[idx]]
+        pose = self.camera_poses[self.timestamps[idx]]
         rgb = np.array(rgb)
 
         if return_dict:
             return dict(image=rgb,
                         depth=depth,
                         pose=pose,
-                        timestamp=self.time_stamps[idx],
+                        timestamp=self.timestamps[idx],
                         idx=idx
                         )
-        return rgb, depth, pose, self.time_stamps[idx], idx
-
-    def get_shape(self):
-        return self.shape
-
-    def read(self):
-        try:
-            data = self.get_frame(self.internal_index, return_dict=True)
-            self.internal_index += 1
-        except:
-            return None, None, None, False
-        return data["image"],  self.internal_index-1, data["timestamp"], True
+        return rgb, depth, pose, self.timestamps[idx], idx
 
     def get_pose(self, idx):
-        return self.camera_poses[self.time_stamps[idx]]
-
-
-def get_default_mp3d_scene():
-    scene = "1LXtFkjw3qL/0"
-    path = "/home/kike/Documents/datasets/MP3D_VO"
-    return MP3D_VO(dt_dir=path, scene=scene)
-
-
-if __name__ == '__main__':
-    basedir = "/home/kike/Documents/datasets/MP3D_VO"
-    scene = "2n8kARJN3HM/0"
-
-    assert os.path.isdir(basedir)
-    dt = MP3D_VO(dt_dir=basedir, scene=scene)
-
-    rgb, depth, pose, step = dt.get_frame(idx=150)
-    plt.subplot(1, 2, 1)
-    plt.imshow(rgb)
-    plt.subplot(1, 2, 2)
-    plt.imshow(depth)
-    plt.show()
-    import cv2
-    cv2.imshow("test", rgb)
-    cv2.waitKey(0)
-    print("done")
+        return self.camera_poses[self.timestamps[idx]]
+
+    def get_pcl(self, idx):
+        """
+        By default, we expect that every dataset has depth information
+        This method return a dense PCL by indexing the frame id
+        """
+        assert self.camera_projection is not None, "camera_projection is not defined"
+        rgb, depth, pose, timestamps, idx = self.get_frame(
+            idx, return_depth=True)
+
+        if depth is None:
+            print("Dataset {} does not have depth information".format(self.dataset_name))
+            return None, None, pose, timestamps, idx
+        pcl, color_pcl = self.camera_projection.get_pcl(
+            color_map=rgb,
+            depth_map=depth,
+            scaler=self.camera_projection.scaler)
+        return pcl, color_pcl, pose, timestamps, idx
+
+    def get_pcl_from_key_features(self, idx, extractor, mask=None):
+        """
+        Returns a set of 3D-landmarks from a set of keyfeatures (keypoints)
+        The projection of landmarks is based on the defined camera model and the
+        depth map associated to the frame (color image) 
+        """
+        assert self.camera_projection is not None, "camera_projection is not defined"
+        frame_dict = self.get_frame(idx, return_dict=True)
+        # ! BY default any extractor return key-points (cv2.KeyPoints) from get_features()
+        key_points = extractor.get_features(frame_dict["image"], mask=mask)
+        pixels = np.array([kp.pt for kp in key_points]).astype(int)
+
+        if frame_dict["depth"] is None:
+            print("Dataset {} does not have depth information".format(self.dataset_name))
+            return None
+
+        depth = frame_dict["depth"][pixels[:, 1], pixels[:, 0]]
+        msk_depth = depth > 0
+        bearings = self.camera_projection.cam.pixel2euclidean_space(pixels)
+        return bearings[:, msk_depth] * depth[msk_depth]

dataset_reader/TUM_VI.py

@@ -1,22 +1,25 @@
+import sys
+sys.path.append("/home/kike/Documents/Research/Utilities")
+sys.path.append("/home/kike/Documents/Research/CameraModels")
+from .dataset import Data
+import matplotlib.pyplot as plt
+from file_utilities import read_yaml_file, load_obj, save_obj
+from geometry_utilities import *
+from vispy_utilities import *
+from sphere import Sphere
 import cv2
 import os
 import numpy as np
-from sphere import Sphere
-from vispy_utilities import *
-from geometry_utilities import *
-from file_utilities import read_yaml_file, load_obj, save_obj
 
-import matplotlib.pyplot as plt
-from read_datasets.data import Data
 
 
 class TUM_VI(Data):
-    def __init__(self, basedir, scene):
+    def __init__(self, dt_dir, scene):
         # Adding dataset utilities
         # print("loading TUM_RGBD_SLAM_data from: {}\nScene: {}".format(dataset_path, scene))
         # TUM_utilities = os.path.join(dataset_path, "utilities/scripts")
         # sys.path.append(TUM_utilities)
-        super().__init__(basedir, scene)
+        super().__init__(dt_dir, scene)
 
         from TUM_RGBD_utils.associate import associate, read_file_list, read_file_association
         from TUM_RGBD_utils.evaluate_rpe import read_trajectory
@@ -29,7 +32,7 @@ def __init__(self, basedir, scene):
         # ! For now we are considering a monocular camera so
         # ! extrinsic parameters are neglected
         # camera_yaml = read_yaml_file(self.scene_dir + '/dso/camchain.yaml')
-        camera_yaml = read_yaml_file(self.basedir + '/omni_calib.yaml')
+        camera_yaml = read_yaml_file(self.dt_dir + '/omni_calib.yaml')
         self.camera_projection = UnifiedModel.by_calibrated_parameters(calib=camera_yaml)
 
         # ! Create dic-bases
@@ -59,14 +62,22 @@ def __init__(self, basedir, scene):
                 pose_rgb_association_file)
 
         self.number_frames = self.dic_pose_rgb_association.shape[0]
-        self.stamps = self.dic_pose_rgb_association[:, 0]
+        self.timestamps = self.dic_pose_rgb_association[:, 0]
         self.t_cam_imu_transform = np.array(camera_yaml["cam0"]['T_cam_imu']).reshape((4, 4))
         self.shape = self.camera_projection.shape
         self.cam = self.camera_projection
 
     def get_shape(self):
         return self.camera_projection.get_shape()
 
+    def get_rgb(self, idx):
+        frame_dict = self.get_frame(idx, return_dict=True)
+        return frame_dict["image"]
+
+    def get_pose(self, idx):
+        frame_dict = self.get_frame(idx, return_dict=True)
+        return frame_dict["pose"]
+
     def get_frame(self, idx=0, return_dict=True):
         pose_timestamp = self.dic_pose_rgb_association[idx, 0]
         image_timestamp = self.dic_pose_rgb_association[idx, 1]
@@ -80,37 +91,8 @@ def get_frame(self, idx=0, return_dict=True):
             return dict(image=rgb,
                         depth=None,
                         pose=pose,
-                        timestamp=self.stamps[idx])
-
-        return rgb, None, pose, self.stamps[idx]
-
-
-if __name__ == "__main__":
-    path = "/home/justin/Downloads"
-    scene = "dataset-room3_512_16"
-    file_dt = "{}.tumvi".format(scene)
-
-    saving_data = True
-
-    if saving_data:
-        data = TUM_VI(basedir=path, scene=scene)
-        save_obj(filename=file_dt, obj=data)
-    else:
-        data = load_obj(filename=file_dt)
-
-    pts = []
-
-    for idx in range(0, data.seq_len, 1):
-        rgb, pose, stamp = data.get_frame(idx)
-        pts.append(pose[:, 3])
-        # print(pts[-1])
-        # cv2.imshow("TUM VI", rgb)
-        # cv2.waitKey(1)
-
-    fig = plt.figure()
-    ax = fig.add_subplot(projection='3d')
-
-    pts = np.array(pts)
-    ax.scatter(pts[:, 0], pts[:, 1], pts[:, 2], c=pts[:, 2], cmap='viridis')
+                        timestamp=self.timestamps[idx],
+                        idx=idx
+                        )
 
-    plt.show()
+        return rgb, None, pose, self.timestamps[idx], idx