Make empty timestamps default everywhere (#430)

* Add timestamps in open3d dataloading, now it is the first option in
generic

* Need o3d>=0.16

* Not used

* Formatting

* Add normalization

* Use empty timestamps everywhere

---------

Co-authored-by: tizianoGuadagnino <frevo93@gmail.com>

Author: Benedikt Mersch

python/kiss_icp/datasets/apollo.py

@@ -49,7 +49,7 @@ def __len__(self):
         return len(self.scan_files)
 
     def __getitem__(self, idx):
-        return self.get_scan(self.scan_files[idx])
+        return self.get_scan(self.scan_files[idx]), np.array([])
 
     def get_scan(self, scan_file: str):
         points = np.asarray(self.o3d.io.read_point_cloud(scan_file).points, dtype=np.float64)

python/kiss_icp/datasets/kitti.py

@@ -54,7 +54,7 @@ def __len__(self):
         return len(self.scan_files)
 
     def scans(self, idx):
-        return self.read_point_cloud(self.scan_files[idx])
+        return self.read_point_cloud(self.scan_files[idx]), np.array([])
 
     def apply_calibration(self, poses: np.ndarray) -> np.ndarray:
         """Converts from Velodyne to Camera Frame"""

python/kiss_icp/datasets/mcap.py

@@ -23,6 +23,8 @@
 import os
 import sys
 
+import numpy as np
+
 
 class McapDataloader:
     def __init__(self, data_dir: str, topic: str, *_, **__):
@@ -58,7 +60,7 @@ def __del__(self):
     def __getitem__(self, idx):
         msg = next(self.msgs).ros_msg
         self.timestamps.append(self.stamp_to_sec(msg.header.stamp))
-        return self.read_point_cloud(msg)
+        return self.read_point_cloud(msg), np.array([])
 
     def __len__(self):
         return self.n_scans

python/kiss_icp/datasets/nclt.py

@@ -54,7 +54,9 @@ def __len__(self):
         return len(self.scan_files)
 
     def __getitem__(self, idx):
-        return self.read_point_cloud(os.path.join(self.scans_dir, self.scan_files[idx]))
+        return self.read_point_cloud(os.path.join(self.scans_dir, self.scan_files[idx])), np.array(
+            []
+        )
 
     def read_point_cloud(self, file_path: str):
         def _convert(x_s, y_s, z_s):

python/kiss_icp/datasets/nuscenes.py

@@ -76,7 +76,7 @@ def __len__(self):
         return len(self.lidar_tokens)
 
     def __getitem__(self, idx):
-        return self.read_point_cloud(self.lidar_tokens[idx])
+        return self.read_point_cloud(self.lidar_tokens[idx]), np.array([])
 
     def read_point_cloud(self, token: str):
         filename = self.nusc.get("sample_data", token)["filename"]

python/kiss_icp/datasets/tum.py

@@ -93,4 +93,4 @@ def __getitem__(self, idx):
                 self.o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault
             ),
         )
-        return np.array(pcd.points, dtype=np.float64)
+        return np.array(pcd.points, dtype=np.float64), np.array([])

python/kiss_icp/pipeline.py

@@ -98,7 +98,7 @@ def run(self):
     # Private interface  ------
     def _run_pipeline(self):
         for idx in get_progress_bar(self._first, self._last):
-            raw_frame, timestamps = self._next(idx)
+            raw_frame, timestamps = self._dataset[idx]
             start_time = time.perf_counter_ns()
             source, keypoints = self.odometry.register_frame(raw_frame, timestamps)
             self.poses[idx - self._first] = self.odometry.last_pose
@@ -114,16 +114,6 @@ def _run_pipeline(self):
                 self._vis_infos,
             )
 
-    def _next(self, idx):
-        """TODO: re-arrange this logic"""
-        dataframe = self._dataset[idx]
-        try:
-            frame, timestamps = dataframe
-        except ValueError:
-            frame = dataframe
-            timestamps = np.array([])
-        return frame, timestamps
-
     @staticmethod
     def save_poses_kitti_format(filename: str, poses: np.ndarray):
         def _to_kitti_format(poses: np.ndarray) -> np.ndarray:

python/kiss_icp/tools/point_cloud2.py

@@ -86,7 +86,7 @@ def read_point_cloud(msg: PointCloud2) -> Tuple[np.ndarray, np.ndarray]:
         max_timestamp = np.max(timestamps)
         timestamps = (timestamps - min_timestamp) / (max_timestamp - min_timestamp)
     else:
-        timestamps = np.ones(points.shape[0])
+        timestamps = np.array([])
     return points.astype(np.float64), timestamps