Add timestamps - KITTI, Rosbag

python/kiss_icp/datasets/kitti.py

@@ -85,6 +85,10 @@ def _lidar_pose_gt(poses_gt):
         poses = poses.reshape((n, 4, 4))  # [N, 4, 4]
         return _lidar_pose_gt(poses)
 
+    def get_frames_timestamps(self) -> np.ndarray:
+        timestamps = np.loadtxt(os.path.join(self.kitti_sequence_dir, "times.txt")).reshape(-1, 1)
+        return timestamps
+
     @staticmethod
     def read_calib_file(file_path: str) -> dict:
         calib_dict = {}
@@ -102,3 +106,4 @@ def read_calib_file(file_path: str) -> dict:
                     key = tokens[0][:-1]
                     calib_dict[key] = values
         return calib_dict
+

python/kiss_icp/datasets/rosbag.py

@@ -46,6 +46,7 @@ def __init__(self, data_dir: Path, topic: str, *_, **__):
         # Get an iterable
         self.n_scans = self.bag.get_message_count(topic_filters=self.topic)
         self.msgs = self.bag.read_messages(topics=[self.topic])
+        self.timestamps = []
 
         # Visualization Options
         self.use_global_visualizer = True
@@ -55,8 +56,12 @@ def __len__(self):
 
     def __getitem__(self, idx):
         # TODO: implemnt [idx], expose field_names
-        _, msg, _ = next(self.msgs)
+        _, msg, time = next(self.msgs)
+        self.timestamps.append(time.to_sec())
         return self.read_point_cloud(msg)
+
+    def get_frames_timestamps(self) -> list:
+        return self.timestamps
 
     def check_topic(self, topic: str) -> str:
         # when user specified the topic don't check