Added RGBD and STEREO. Improvements to datasets management. Improvements

to ground thruth management and viz. Added map saving and reloading.
Faste. Better.

.gitignore

@@ -21,5 +21,7 @@ videos/webcam
 kf_info.log 
 local_mapping.log 
 tum_association_matches.txt 
+kitti_trajectory.txt
+map.json
 
 thirdparty/protoc 

TROUBLESHOOTING.md

@@ -1,22 +1,41 @@
 # Troubleshooting 
 
+<!-- TOC -->
+
+- [Troubleshooting](#troubleshooting)
+  - [Bad tracking performances](#bad-tracking-performances)
+  - [Gtk-ERROR \*\*: ... GTK+ 2.x symbols detected. Using GTK+ 2.x and GTK+ 3 in the same process is not supported](#gtk-error---gtk-2x-symbols-detected-using-gtk-2x-and-gtk-3-in-the-same-process-is-not-supported)
+  - [SURF error](#surf-error)
+  - [g2o Errors](#g2o-errors)
+    - [AttributeError: 'g2o.EdgeSE3ProjectXYZ' object has no attribute 'fx'](#attributeerror-g2oedgese3projectxyz-object-has-no-attribute-fx)
+    - [Cannot properly import g2o library or other libs](#cannot-properly-import-g2o-library-or-other-libs)
+  - [When loading a neural network with CUDA everything gets stuck](#when-loading-a-neural-network-with-cuda-everything-gets-stuck)
+  - [OrderedSet](#orderedset)
+  - [Problems with ROS and OpenCV](#problems-with-ros-and-opencv)
+  - [Could not import PILLOW\_VERSION from PIL](#could-not-import-pillow_version-from-pil)
+  - [ValueError: ndarray is not C-contiguous](#valueerror-ndarray-is-not-c-contiguous)
+  - [Error: python3: malloc.c:2401: sysmalloc: Assertion \`(old\_top == initial\_top (av) \&\& old\_size == 0) ...](#error-python3-mallocc2401-sysmalloc-assertion-old_top--initial_top-av--old_size--0-)
+  - [Python version](#python-version)
+
+<!-- /TOC -->
+
 This page contains a small collections of issues/errors that may be experienced along with their fixes. 
 
 **FIRST OF ALL**: did you read the main [README](./README.md) page? did you use the provided **INSTALL SCRIPTS**? If not then go back on the [README](./README.md) page, read the few lines in the install section and launch the **REQUIRED** install script. The install scripts were created in order to perform all the required install operations for you and make the install process itself as painless as possible.   
 
 If you work under **Ubuntu 20.04** or **MacOS**, check the specific installation procedures reported in the main [README](./README.md) page. 
 
 
-### Bad tracking performances
+## Bad tracking performances
 
 Due to the multi-threading system (tracking thread + local mapping thread) and the non-super-fast performances of the python implementations (indeed, [python is not actually multithreading](https://www.theserverside.com/blog/Coffee-Talk-Java-News-Stories-and-Opinions/Is-Pythons-GIL-the-software-worlds-biggest-blunder#:~:text=Python%20multithreading&text=Python's%20Global%20Interpreter%20Lock%20(GIL,the%20direction%20computer%20hardware%20took.)), bad tracking performances may occur and vary depending on your machine computation capabilities. In a few words, it may happen that the local mapping thread is not fast enough to spawn new map points in time for the tracking thread. In fact, new spawned map points are necessary to let the tracking thread find enough {keypoint}-{map point} correspondences, and hence stably grasp at the map and proceed along its estimated trajectory. Simply put, the local mapping thread continuously builds/unrolls the fundamental 'carpet' of points (the map) on which the tracking thread 'walks': no 'carpet', no party!
 
 If you experience bad tracking performances, go in [parameters.py](./parameters.py) and: 
-1) first, try to increase/adjust the parameter `kTrackingWaitForLocalMappingSleepTime`
+1) first, try to increase/adjust the parameter `kTrackingWaitForLocalMappingSleepTimeMono` or `kTrackingWaitForLocalMappingSleepTimeStereo` (depending on the used dataset)
 
 2) then, if you don't actually see any satisfying improvement with step (1), set `kTrackingWaitForLocalMappingToGetIdle=True`
 
-### Gtk-ERROR **: ... GTK+ 2.x symbols detected. Using GTK+ 2.x and GTK+ 3 in the same process is not supported
+## Gtk-ERROR **: ... GTK+ 2.x symbols detected. Using GTK+ 2.x and GTK+ 3 in the same process is not supported
 
 If you hit such an error then decomment (or add) the following code in both `main_vo.py` and `main_slam.py`
 ```
@@ -26,7 +45,7 @@ gi.require_version('Gtk', '2.0')
 ```
 this will solve the problem. 
 
-### SURF error
+## SURF error
 
 In order to use [non-free OpenCV features](https://stackoverflow.com/questions/50467696/pycharm-installation-of-non-free-opencv-modules-for-operations-like-sift-surf) (i.e. **SURF**, etc.), you need to install the module `opencv-contrib-python` built with the enabled option `OPENCV_ENABLE_NONFREE`. You can find SURF availalble in `opencv-contrib-python 3.4.2.16`: this can be installed by running
 ```
@@ -40,7 +59,9 @@ How to check your installed OpenCV version:
 $ python3 -c "import cv2; print(cv2.__version__)"
 ```
 
-### g2o Error
+## g2o Errors
+
+### AttributeError: 'g2o.EdgeSE3ProjectXYZ' object has no attribute 'fx'
 
 If you run into the following error
 ```
@@ -72,15 +93,17 @@ First of all, check if you have a compiled `thirdparty/g2opy/lib/g2o.cpython-*-l
 
 On the other hand, if you already have a compiled `thirdparty/g2opy/lib/g2o.cpython-*-linux-gnu.so`, it's very likely you have libraries compiled in a 'mixed' way. Then, try to clean everything with the script `clean.sh`, and follow the installation procedure again (see the main [README](./README.md) file). 
 
-### When loading a neural network with CUDA everything gets stuck
+Last but not least, please recall that you need to activate your `pyenv`/`conda` environment before launching any pySLAM script.
+
+## When loading a neural network with CUDA everything gets stuck
 
 I got this issue with a new NVIDIA GPU while loading `SuperPoint` neural network. The NN loading got stuck. This error arises when CUDA code was not compiled to target your GPU architecture. Two solutions: 
 - Easy: turn off CUDA (for instance, with `SuperPointFeature2D()` set the default class option `do_cuda=False`). In this case, the computations will be moved to CPU. 
 - You need to install a pytorch version that is compatible with your CUDA version and GPU architecture. See for instance these two links: 
 https://stackoverflow.com/questions/75682385/runtimeerror-cuda-error-no-kernel-image-is-available-for-execution-on-the-devi
 https://discuss.pytorch.org/t/failed-to-load-image-python-extension-could-not-find-module/140278   
 
-### OrderedSet 
+## OrderedSet 
 
 reference https://github.com/luigifreda/pyslam/issues/48 
 
@@ -97,7 +120,7 @@ You can solve such an issue by installing a lower version of OrderedSet
 pip install ordered-set==3.1.1 --force-reinstall
 ``` 
 
-### Problems with ROS and OpenCV
+## Problems with ROS and OpenCV
 
 If you have ROS installed in your system and got the following error:
 ```
@@ -111,7 +134,7 @@ $ export PYTHONPATH=""
 this will remove the ROS OpenCV python modules from your python path and will solve the issue. 
 
 
-### Could not import PILLOW_VERSION from PIL 
+## Could not import PILLOW_VERSION from PIL 
 
 Or **ImportError: cannot import name 'PILLOW_VERSION'**
 
@@ -124,7 +147,7 @@ $ pip3 install pillow==6.2.2
 (fix from this [page](https://stackoverflow.com/questions/59659146/could-not-import-pillow-version-from-pil))
 
 
-### ValueError: ndarray is not C-contiguous
+## ValueError: ndarray is not C-contiguous
 
 If the following error pops-up:
 ```
@@ -143,7 +166,7 @@ projs = projs.copy(order='C')
 
 (thanks to [naughtyStark](https://github.com/naughtyStark))
 
-### Error: python3: malloc.c:2401: sysmalloc: Assertion `(old_top == initial_top (av) && old_size == 0) ...
+## Error: python3: malloc.c:2401: sysmalloc: Assertion `(old_top == initial_top (av) && old_size == 0) ...
 
 I got this error after messing up with the installation of different python packages related to torch and torchvision. The result was that tfeat was generating segmentation faults. In order to check if this is actually your case, run
 ```
@@ -159,6 +182,6 @@ in order to get a clean installation of the torch packages.
 
 
 
-### Python version
+## Python version
 
-Installation issues may happen if multiple python versions are mixed. All the instructions reported in this repository assume you are using python3. If you really want to install things manually instead of using the install scripts, follow the same steps of the install scripts and be sure to use pip3 instead of pip, and good luck!
+Installation issues may happen if multiple python versions are mixed. All the instructions reported in this repository assume you are using python3. If you really want to install things manually instead of using the install scripts, follow the same steps of the install scripts, and good luck!

camera.py

@@ -17,41 +17,171 @@
 * along with PYSLAM. If not, see <http://www.gnu.org/licenses/>.
 """
 
+from enum import Enum
 import numpy as np 
 import cv2
+import json
 #import g2o
 from utils_geom import add_ones
+from utils_sys import Printer
 
+class CameraTypes(Enum):
+    NONE = 0
+    PINHOLE = 1
+
+class CameraBase:
+    def __init__(self):
+        self.type = CameraTypes.NONE
+        self.width, self.height = None, None
+        self.fx, self.fy = None, None
+        self.cx, self.cy = None, None
+
+        self.D = None
+        self.is_distorted = None
+
+        self.fps = None
+
+        self.bf = None
+        self.b = None        
+
+        self.u_min = None 
+        self.u_max = None
+        self.v_min = None
+        self.v_max = None
+        self.initialized = False   
+
+class Camera(CameraBase): 
+    def __init__(self, config):
+        super().__init__()
+        if config is None:
+            return 
+        width = config.cam_settings['Camera.width']
+        height = config.cam_settings['Camera.height']
+        fx = config.cam_settings['Camera.fx']
+        fy = config.cam_settings['Camera.fy']
+        cx = config.cam_settings['Camera.cx']
+        cy = config.cam_settings['Camera.cy']
+        D = config.DistCoef # D = [k1, k2, p1, p2, k3]
+        fps = config.cam_settings['Camera.fps']
 
-class Camera: 
-    def __init__(self, width, height, fx, fy, cx, cy, D, fps = 1): # D = [k1, k2, p1, p2, k3]
         self.width = width
         self.height = height
         self.fx = fx
         self.fy = fy
         self.cx = cx
         self.cy = cy
+
         self.D = np.array(D,dtype=np.float32) # np.array([k1, k2, p1, p2, k3])  distortion coefficients 
+        self.is_distorted = np.linalg.norm(self.D) > 1e-10
+
         self.fps = fps 
+
+        # If stereo camera => assuming rectified images as input at present (so no need of left-right transformation matrix Tlr)     
+        if 'Camera.bf' in config.cam_settings:
+            self.bf = config.cam_settings['Camera.bf']
+            self.b = self.bf/self.fx
+        if config.sensor_type == 'stereo' and self.bf is None:
+            raise ValueError('Expecting the field Camera.bf in the camera config file')
+        self.depth_factor = 1.0 # Deptmap values factor 
+        if 'DepthMapFactor' in config.cam_settings:
+            self.depth_factor = 1.0/float(config.cam_settings['DepthMapFactor'])
+            print('Using DepthMapFactor = %f' % self.depth_factor)
+        if config.sensor_type == 'rgbd' and self.depth_factor is None:
+            raise ValueError('Expecting the field DepthMapFactor in the camera config file')            
+        self.depth_threshold = None  # Close/Far threshold. Baseline times.
+        if 'ThDepth' in config.cam_settings:
+            depth_threshold = float(config.cam_settings['ThDepth'])
+            assert(self.bf is not None)
+            self.depth_threshold = self.bf * depth_threshold / self.fx 
+            print('Using depth_threshold = %f' % self.depth_threshold)
+        if (config.sensor_type == 'rgbd' or config.sensor_type == 'stereo') and self.depth_threshold is None:
+            raise ValueError('Expecting the field ThDepth in the camera config file')              
 
-        self.is_distorted = np.linalg.norm(self.D) > 1e-10
-        self.initialized = False     
+
+    def is_stereo(self):
+        return self.bf is not None
+
+    def to_json(self):
+        return {'type':int(self.type.value),
+                'width':int(self.width), 
+                'height':int(self.height), 
+                'fx':float(self.fx), 
+                'fy':float(self.fy), 
+                'cx':float(self.cx), 
+                'cy':float(self.cy), 
+                'D':json.dumps(self.D.astype(float).tolist() if self.D is not None else None), 
+                'fps':int(self.fps),
+                'bf':float(self.bf),
+                'b':float(self.b),
+                'depth_factor':float(self.depth_factor),
+                'depth_threshold':float(self.depth_threshold),
+                'is_distorted':bool(self.is_distorted),
+                'u_min':float(self.u_min),
+                'u_max':float(self.u_max),
+                'v_min':float(self.v_min),
+                'v_max':float(self.v_max),
+                'initialized':bool(self.initialized)
+                }
 
+    def init_from_json(self, json_str):
+        self.type = CameraTypes(int(json_str['type']))
+        self.width = int(json_str['width'])
+        self.height = int(json_str['height'])
+        self.fx = float(json_str['fx'])
+        self.fy = float(json_str['fy'])
+        self.cx = float(json_str['cx'])
+        self.cy = float(json_str['cy'])
+        self.D = np.array(json.loads(json_str['D'])) if json_str['D'] is not None else None
+        self.fps = int(json_str['fps'])
+        self.bf = float(json_str['bf'])
+        self.b = float(json_str['b'])
+        self.depth_factor = float(json_str['depth_factor'])
+        self.depth_threshold = float(json_str['depth_threshold'])
+        self.is_distorted = bool(json_str['is_distorted'])
+        self.u_min = float(json_str['u_min'])
+        self.u_max = float(json_str['u_max'])
+        self.v_min = float(json_str['v_min'])
+        self.v_max = float(json_str['v_max'])
+        self.initialized = bool(json_str['initialized'])
 
 class PinholeCamera(Camera):
-    def __init__(self, width, height, fx, fy, cx, cy, D, fps = 1):
-        super().__init__(width, height, fx, fy, cx, cy, D, fps)
+    def __init__(self, config):
+        super().__init__(config)
+        self.type = CameraTypes.PINHOLE
+
+        if config is None:
+            return 
+
+        fx = self.fx 
+        fy = self.fy
+        cx = self.cx
+        cy = self.cy
         self.K = np.array([[fx, 0,cx],
                            [ 0,fy,cy],
                            [ 0, 0, 1]])
         self.Kinv = np.array([[1/fx,    0,-cx/fx],
                               [   0, 1/fy,-cy/fy],
                               [   0,    0,    1]])             
 
+        #print(f'PinholeCamera: K = {self.K}')
         self.u_min, self.u_max = 0, self.width 
         self.v_min, self.v_max = 0, self.height       
         self.init()    
 
+    def to_json(self):
+        camera_json = super().to_json()
+        camera_json['K'] = json.dumps(self.K.astype(float).tolist())
+        camera_json['Kinv'] = json.dumps(self.Kinv.astype(float).tolist())
+        return camera_json
+
+    @staticmethod
+    def from_json(json_str):
+        c = PinholeCamera(None)
+        c.init_from_json(json_str)
+        c.K = np.array(json.loads(json_str['K']))
+        c.Kinv = np.array(json.loads(json_str['Kinv']))
+        return c
+
     def init(self):
         if not self.initialized:
             self.initialized = True 
@@ -60,12 +190,26 @@ def init(self):
     # project a 3D point or an array of 3D points (w.r.t. camera frame), of shape [Nx3]
     # out: Nx2 image points, [Nx1] array of map point depths     
     def project(self, xcs):
-        #u = self.fx * xc[0]/xc[2] + self.cx
-        #v = self.fy * xc[1]/xc[2] + self.cy  
-        projs = self.K @ xcs.T     
+        # u = self.fx * xc[0]/xc[2] + self.cx
+        # v = self.fy * xc[1]/xc[2] + self.cy  
+        projs = self.K @ xcs.T
         zs = projs[-1]      
         projs = projs[:2]/ zs   
         return projs.T, zs
+
+    # stereo-project a 3D point or an array of 3D points (w.r.t. camera frame), of shape [Nx3]
+    # (assuming rectified stereo images)
+    # out: Nx3 image points, [Nx1] array of map point depths     
+    def project_stereo(self, xcs):
+        # u = self.fx * xc[0]/xc[2] + self.cx
+        # v = self.fy * xc[1]/xc[2] + self.cy  
+        # ur = u - bf//xc[2]
+        projs = self.K @ xcs.T     
+        zs = projs[-1]      
+        projs = projs[:2]/ zs 
+        ur = projs[0] - self.bf/zs
+        projs = np.concatenate((projs.T,ur[:, np.newaxis]),axis=1)  
+        return projs, zs    
 
     # unproject 2D point uv (pixels on image plane) on 
     def unproject(self, uv):
@@ -74,10 +218,15 @@ def unproject(self, uv):
         return x,y
 
     # in:  uvs [Nx2]
-    # out: xcs array [Nx3] of normalized coordinates     
+    # out: xcs array [Nx2] of normalized coordinates     
     def unproject_points(self, uvs):
         return np.dot(self.Kinv, add_ones(uvs).T).T[:, 0:2]        
 
+    # in:  uvs [Nx2], depths [Nx1]
+    # out: xcs array [Nx3] of normalized coordinates     
+    def unproject_points_3d(self, uvs, depths):
+        return np.dot(self.Kinv, add_ones(uvs).T * depths).T[:, 0:3]    
+
     # in:  uvs [Nx2]
     # out: uvs_undistorted array [Nx2] of undistorted coordinates  
     def undistort_points(self, uvs):