Merge branch 'master' into CT_ICP

Author: pdell-kitware

README.md

@@ -2,6 +2,7 @@
 
 This codebase proposes modular light python and pytorch implementations of several LiDAR Odometry methods, 
 which can easily be evaluated and compared on a set of public Datasets.
+![pyLiDAR-SLAM LiDAR Odometry modules](docs/data/overview.png)
 
 It heavily relies on [omegaconf](https://omegaconf.readthedocs.io/en/2.0_branch/) and [hydra](https://hydra.cc/), 
 which allows us to easily test the different modules and parameters with few but structured configuration files.
@@ -36,8 +37,10 @@ The goal for the future is to gradually add functionalities to pyLIDAR-SLAM (Loo
     ├─ models              # PoseNet model code 
     ├─ odometry            # Odometry modules 
     ├─ training            # Modules for training PoseNet models 
-    ├─ backend             # Work in Progress
-    ├─ loop_closure        # Work in Progress
+    ├─ initialization.py   # Modules for Initial motion estimate 
+    ├─ preprocessing.py    # Modules for Preprocessing the point cloud 
+    ├─ backend.py          # Work in Progress
+    ├─ loop_closure.py     # Work in Progress
     └─ viz                 # Tools for visualization 
 
 ├─ tests
@@ -73,9 +76,9 @@ The configuration hierarchy in this project follows the hierarchy of folder `con
 
 >- **Group** [`dataset`](slam/dataset/dataset.py):
 >   - configurations: [`kitti`](slam/dataset/kitti_dataset.py), [`nclt`](slam/dataset/nclt_dataset.py), [`ford_campus`](slam/dataset/ford_campus.py)
->- **Group** [`odometry/initialization`](slam/odometry/initialization.py) (Initialization module for the Frame To Model):
->   - configurations: [`EI`](slam/odometry/initialization.py), [`CV`](slam/odometry/initialization.py), [`PoseNet`](slam/odometry/initialization.py)
->- **Group** [`odometry/local_map`](slam/odometry/local_map.py) (The local map Implementation):
+>- **Group** [`slam/initialization`](slam/initialization.py) (Initialization module for the Frame To Model):
+>   - configurations: [`EI`](slam/initialization.py), [`CV`](slam/initialization.py), [`PoseNet`](slam/initialization.py)
+>- **Group** [`slam/odometry/local_map`](slam/odometry/local_map.py) (The local map Implementation):
 >   - configurations [`projective`](slam/odometry/local_map.py), [`kdtree`](slam/odometry/local_map.py), 
 
 ```bash
@@ -89,13 +92,19 @@ The configuration hierarchy in this project follows the hierarchy of folder `con
         ├─ odometry         
             ├─ alignment                # Alignment Group (will be expended in the future)
                 └─ point_to_plane_GN.yaml   # Point-to-Plane alignment for the Frame-to-Model
-            ├─ initialization           # Initialization Group
-                ├─ CV.yaml                  # Configuration for the constant velocity model
-                ├─ PoseNet.yaml             # Configuration for using PoseNet as initialization
-                └─ EI.yaml                  # Elevation Image 2D registration configuration
             └─ local_map                # The Local Map Group
                 ├─ projective               # The projective Frame-to-Model proposed
-                └─ kdtree                   # The kdtree based Frame-to-Model alignemnt 
+                └─ kdtree                   # The kdtree based Frame-to-Model alignmeeent 
+        ├─ preprocessing            # Preprocessing Group
+            ├─ grid_sample            
+            ├─ voxelization            
+               ...
+
+        ├─ initialization           # Initialization Group
+            ├─ CV.yaml                  # Configuration for the constant velocity model
+            ├─ PoseNet.yaml             # Configuration for using PoseNet as initialization
+            └─ EI.yaml                  # Elevation Image 2D registration configuration
+            
         ├─ loop_closure                 # Loop Closure Group
         └─ backend                      # Backend Group
 
@@ -156,7 +165,7 @@ export KITTI_ODOM_ROOT=<path-to-kitti-odometry-root-directory>     # The path to
 
 # Run the script
 python run.py dataset=kitti num_workers=4 device=cuda:0 slam/odometry/local_map=projective \
-       slam/odometry/initialization=CV slam.odometry.local_map.num_neighbors_normals=15
+       slam/initialization=CV slam.odometry.local_map.num_neighbors_normals=15
 ```
 
 The output files (configuration files, logs, and optionally metrics on the trajectory) will be saved by default at location : 
@@ -209,9 +218,9 @@ In [benchmark.md](docs/benchmark.md) we present the results of *pyLIDAR-SLAM* on
 
 ### System Tested
 
-| OS            | CUDA   | pytorch  | python | 
-| --- | --- | --- | --- | 
-| Ubuntu 18.04  | 10.2   | 1.7.1    | 3.8.8 |
+| OS            | CUDA   | pytorch  | python | hydra |
+| --- | --- | --- | --- | --- |
+| Ubuntu 18.04  | 10.2   | 1.7.1    | 3.8.8 | 1.0 |
 
 ### Author
 This is a work realised in the context of Pierre Dellenbach PhD thesis under supervision of [Bastien Jacquet](https://www.linkedin.com/in/bastienjacquet/?originalSubdomain=fr) ([Kitware](https://www.kitware.com/computer-vision/)), 

config/slam.yaml

@@ -24,6 +24,7 @@ defaults:
   - slam/odometry: icp_odometry
   - dataset: kitti
   - slam/preprocessing: none
+  - slam/initialization: CV
   - slam/loop_closure: none
   - slam/backend: none
   - hydra/output: slam

config/slam/odometry/initialization/CV.yaml config/slam/initialization/CV.yaml

@@ -1,11 +1,21 @@
 # @package _group_
 
 filters:
+
   "1":
+    filter_name: distortion
+    force: false                          # Whether to fail if timestamps cannot be found
+    activate: true                        # Distortion is only activated if this activate=true
+    pointcloud_key: numpy_pc              # The point cloud key in the dict
+    timestamps_key: numpy_pc_timestamps   # The timestamps key in the dict
+    output_key: distorted
+
+  "2":
     filter_name: grid_sample
     voxel_size: 0.3
+    pointcloud_key: distorted
 
-  "2":
+  "3":
     filter_name: to_tensor
     keys:
       sample_points: input_data

config/slam/odometry/initialization/EI.yaml config/slam/initialization/EI.yaml

@@ -26,14 +26,14 @@ For each component, multiple algorithms are implemented, (more will be added in
 > - **EI** (for Elevation Images): the motion is predicted using a 2D image feature based registration.
 > - **NI** (for No Initialization)
 
-> To select the initialization module, pass in argument `slam/odometry/initialization=<init_name>` where `init_name` is one of `CV,EI,PoseNet,NI`.
+> To select the initialization module, pass in argument `slam/initialization=<init_name>` where `init_name` is one of `CV,EI,PoseNet,NI`.
 
-> All initialization models can be found in [initialization.py](../slam/odometry/initialization.py).
+> All initialization models can be found in [initialization.py](../slam/initialization.py).
 #### Constant Velocity
 > This model is typically the default model for a LiDAR mounted on a car (which has enough inertia for this model to hold).
 > For other acquisition methods (hand-held or mounted on a segway or a drone which lead to abrupt rotations, this might not be a good enough model)
 
-- **Command arguments**: `slam/odometry/initialization=CV`
+- **Command arguments**: `slam/initialization=CV`
 
 #### Elevation Images
 > The relative motion of a sensor is often strictly planar. Our **Elevation Images** based initializes the motion by estimating this 2D motion using Image Registration.
@@ -48,9 +48,9 @@ For each component, multiple algorithms are implemented, (more will be added in
 
 > **Note:** This method requires the OpenCV package `cv2` to be installed (e.g. via `pip install opencv-python`).
 >
-- **Command arguments**: `slam/odometry/initialization=EI` (to visualize the 2D alignments with opencv :`slam.odometry.initialization.debug=true`)
+- **Command arguments**: `slam/initialization=EI` (to visualize the 2D alignments with opencv :`slam.odometry.initialization.debug=true`)
 
-> See [`EIConfig`](../slam/odometry/initialization.py) and [`ElevationImageRegistration`](../slam/common/registration.py) for more details on the configuration options.
+> See [`EIConfig`](../slam/initialization.py) and [`ElevationImageRegistration`](../slam/common/registration.py) for more details on the configuration options.
 
 #### PoseNet
 
@@ -80,7 +80,7 @@ export KITTI_ODOM_ROOT=<path-to-kitti-odometry-root-directory>     # The path to
 export TRAIN_DIR=<absolute-path-to-the-desired-train-dir>          # Path to the output models 
 
 # Launches the Training of PoseNet
-python run.py +device=cuda:0 +num_workers=4 dataset=kitti slam/odometry/initialization=PoseNet
+python run.py +device=cuda:0 +num_workers=4 dataset=kitti slam/initialization=PoseNet
 ```
 
 > See our paper [What's In My LiDAR Odometry Toolbox](https://arxiv.org/abs/2103.09708) for an in depth discussion of the relevance/interest of PoseNet. To summarize: beware of its lack of generalization capacities.
@@ -95,7 +95,7 @@ python run.py +device=cuda:0 +num_workers=4 dataset=kitti slam/odometry/initiali
 > - **none** by default
 
 
-> **Command argument**: `slam/preprocessing=<module name>`. See [preprocessing.py](../slam/preprocessing/preprocessing.py) for more details.
+> **Command argument**: `slam/preprocessing=<module name>`. See [preprocessing.py](../slam/preprocessing.py) for more details.
 
 # Odometry 
 

config/slam/odometry/initialization/PoseNet.yaml config/slam/initialization/PoseNet.yaml

@@ -176,8 +176,7 @@ def __getitem__(self, idx) -> dict:
         data_dict = {}
 
         xyz_r = kitti_read_scan(str(self.lidar_path / f"{idx:010}.bin"))
-        data_dict["numpy_pc"] = self._correct_scan(xyz_r[:, :3])  # xyz_r[:, :3]
-
+        data_dict["numpy_pc"] = KITTIOdometrySequence.correct_scan(xyz_r[:, :3])
         data_dict["numpy_reflectance"] = xyz_r[:, 3:]
         data_dict["numpy_pc_timestamps"] = estimate_timestamps(xyz_r[:, :3], phi_0=np.pi, clockwise=True)
         if self.gt_poses is not None:

config/slam/preprocessing/cv_distortion.yaml

@@ -199,7 +199,7 @@ def __len__(self):
         return self.size
 
     @staticmethod
-    def _correct_scan(scan: np.ndarray):
+    def correct_scan(scan: np.ndarray):
         """
         Corrects the calibration of KITTI's HDL-64 scan
         """
@@ -242,7 +242,7 @@ def __getitem__(self, idx) -> dict:
             assert_debug(scan_path.exists() and scan_path.is_file(), f"The file {scan_path} does not exist")
             scan = kitti_read_scan(str(scan_path))
             # Apply Rectification on the scan
-            scan = self._correct_scan(scan)
+            scan = self.correct_scan(scan)
             if self._with_numpy_pc:
                 data_dict["numpy_pc"] = scan.copy()
             scan = torch.from_numpy(scan[:, :3]).unsqueeze(0)

config/slam/preprocessing/grid_sample.yaml

@@ -59,36 +59,42 @@ class RosbagDataset(IterableDataset):
             topic_mapping (dict): The mapping topic name to key in the data_dict
         """
 
+        def _lazy_initialization(self):
+            if not self.initialized:
+                logging.info(f"[RosbagDataset]Loading ROSBAG {self.file_path}. May take some time")
+                self.rosbag = rosbag.Bag(self.file_path, "r")
+                logging.info(f"Done.")
+
+                topic_info = self.rosbag.get_type_and_topic_info()
+                for topic in self.topic_mapping:
+                    assert_debug(topic in topic_info.topics,
+                                 f"{topic} is not a topic of the rosbag "
+                                 f"(existing topics : {list(topic_info.topics.keys())}")
+                self._len = self.rosbag.get_message_count(self.main_topic) // self._frame_size
+                self.initialized = True
+
         def __init__(self, config: RosbagConfig, file_path: str, main_topic: str, frame_size: int,
                      topic_mapping: Optional[dict] = None):
             self.config = config
             self.rosbag = None
+            self.initialized = False
             assert_debug(Path(file_path).exists(), f"The path to {file_path} does not exist.")
-            logging.info(f"Loading ROSBAG {file_path}")
-            self.rosbag = rosbag.Bag(file_path, "r")
-            logging.info(f"Done.")
-
+            self.file_path = file_path
             self.topic_mapping = topic_mapping if topic_mapping is not None else {}
             if main_topic not in self.topic_mapping:
                 self.topic_mapping[main_topic] = "numpy_pc"
-
-            topic_info = self.rosbag.get_type_and_topic_info()
-            for topic in self.topic_mapping:
-                assert_debug(topic in topic_info.topics,
-                             f"{topic} is not a topic of the rosbag "
-                             f"(existing topics : {list(topic_info.topics.keys())}")
-
-            self.main_topic = main_topic
+            self.main_topic: str = main_topic
+            self.frame_size = frame_size
             self._frame_size: int = frame_size if self.config.accumulate_scans else 1
-            self._len = self.rosbag.get_message_count(self.main_topic) // self._frame_size
-
-            self.__idx = 0
+            self._len = -1  #
+            self._idx = 0
             self._topics = list(topic_mapping.keys())
             self.__iter = None
 
         def __iter__(self):
+            self._lazy_initialization()
             self.__iter = self.rosbag.read_messages(self._topics)
-            self.__idx = 0
+            self._idx = 0
             return self
 
         @staticmethod
@@ -120,7 +126,8 @@ def _save_topic(self, data_dict, key, topic, msg, t, **kwargs):
                 data_dict[timestamps_key].append(timestamps)
 
         def __getitem__(self, index) -> dict:
-            assert_debug(index == self.__idx, "A RosbagDataset does not support Random access")
+            self._lazy_initialization()
+            assert_debug(index == self._idx, "A RosbagDataset does not support Random access")
             assert isinstance(self.config, RosbagConfig)
             if self.__iter is None:
                 self.__iter__()
@@ -134,15 +141,16 @@ def __getitem__(self, index) -> dict:
                 _key = self.topic_mapping[topic]
                 self._save_topic(data_dict, _key, topic, msg, t, frame_index=index)
 
-            self.__idx += 1
+            self._idx += 1
             # Aggregate data
             data_dict = self.aggregate_messages(data_dict)
             return data_dict
 
         def __next__(self):
-            return self[self.__idx]
+            return self[self._idx]
 
         def __len__(self):
+            self._lazy_initialization()
             return self._len
 
         def __del__(self):