Remove ugly unncessary python list appends

python/kiss_icp/datasets/paris_luco.py

@@ -63,9 +63,6 @@ def load_gt_poses(self, file_path):
 
     def apply_calibration(self, poses):
         """ParisLucoDataset only has a x, y, z trajectory, so we must will em all"""
-        new_poses = []
-        for pose in poses:
-            T = pose.copy()
-            T[:3, :3] = np.eye(3)
-            new_poses.append(T)
+        new_poses = poses.copy()
+        poses[:, :3, :3] = np.eye(3)
         return new_poses

python/kiss_icp/metrics.py

@@ -20,20 +20,20 @@
 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
-from typing import List, Tuple
+from typing import Tuple
 
 import numpy as np
 
 from kiss_icp.pybind import kiss_icp_pybind
 
 
-def sequence_error(gt_poses: np.ndarray, results_poses: List[np.ndarray]) -> Tuple[float, float]:
+def sequence_error(gt_poses: np.ndarray, results_poses: np.ndarray) -> Tuple[float, float]:
     """Sptis the sequence error for a given trajectory in camera coordinate frames."""
     return kiss_icp_pybind._kitti_seq_error(gt_poses, results_poses)
 
 
 def absolute_trajectory_error(
-    gt_poses: np.ndarray, results_poses: List[np.ndarray]
+    gt_poses: np.ndarray, results_poses: np.ndarray
 ) -> Tuple[float, float]:
     """Sptis the sequence error for a given trajectory in camera coordinate frames."""
     return kiss_icp_pybind._absolute_trajectory_error(gt_poses, results_poses)

python/kiss_icp/pipeline.py

@@ -25,7 +25,7 @@
 import os
 import time
 from pathlib import Path
-from typing import List, Optional
+from typing import Optional
 
 import numpy as np
 from pyquaternion import Quaternion
@@ -64,8 +64,8 @@ def __init__(
         # Pipeline
         self.odometry = KissICP(config=self.config)
         self.results = PipelineResults()
-        self.times = []
-        self.poses = []
+        self.times = np.zeros(self._n_scans)
+        self.poses = np.zeros((self._n_scans, 4, 4))
         self.has_gt = hasattr(self._dataset, "gt_poses")
         self.gt_poses = self._dataset.gt_poses[self._first : self._last] if self.has_gt else None
         self.dataset_name = self._dataset.__class__.__name__
@@ -97,8 +97,8 @@ def _run_pipeline(self):
             raw_frame, timestamps = self._next(idx)
             start_time = time.perf_counter_ns()
             source, keypoints = self.odometry.register_frame(raw_frame, timestamps)
-            self.poses.append(self.odometry.last_pose)
-            self.times.append(time.perf_counter_ns() - start_time)
+            self.poses[idx - self._first] = self.odometry.last_pose
+            self.times[idx - self._first] = time.perf_counter_ns() - start_time
             self.visualizer.update(source, keypoints, self.odometry.local_map, self.poses[-1])
 
     def _next(self, idx):
@@ -112,22 +112,23 @@ def _next(self, idx):
         return frame, timestamps
 
     @staticmethod
-    def save_poses_kitti_format(filename: str, poses: List[np.ndarray]):
+    def save_poses_kitti_format(filename: str, poses: np.ndarray):
         def _to_kitti_format(poses: np.ndarray) -> np.ndarray:
-            return np.array([np.concatenate((pose[0], pose[1], pose[2])) for pose in poses])
+            return poses[:, :3].reshape(-1, 12)
 
         np.savetxt(fname=f"{filename}_kitti.txt", X=_to_kitti_format(poses))
 
     @staticmethod
     def save_poses_tum_format(filename, poses, timestamps):
         def _to_tum_format(poses, timestamps):
-            tum_data = []
+            tum_data = np.zeros((len(poses), 8))
             with contextlib.suppress(ValueError):
                 for idx in range(len(poses)):
-                    tx, ty, tz = poses[idx][:3, -1].flatten()
+                    tx, ty, tz = poses[idx, :3, -1].flatten()
                     qw, qx, qy, qz = Quaternion(matrix=poses[idx], atol=0.01).elements
-                    tum_data.append([float(timestamps[idx]), tx, ty, tz, qx, qy, qz, qw])
-            return np.array(tum_data).astype(np.float64)
+                    tum_data[idx] = np.r_[float(timestamps[idx]), tx, ty, tz, qx, qy, qz, qw]
+                tum_data.flatten()
+                return tum_data.astype(np.float64)
 
         np.savetxt(fname=f"{filename}_tum.txt", X=_to_tum_format(poses, timestamps), fmt="%.4f")
 
@@ -142,7 +143,7 @@ def _get_frames_timestamps(self):
         return (
             self._dataset.get_frames_timestamps()
             if hasattr(self._dataset, "get_frames_timestamps")
-            else np.arange(0, len(self.poses), 1.0)
+            else np.arange(0, self._n_scans, 1.0)
         )
 
     def _save_poses(self, filename: str, poses, timestamps):
@@ -178,8 +179,8 @@ def _run_evaluation(self):
 
         # Run timing metrics evaluation, always
         def _get_fps():
-            total_time_s = sum(self.times) * 1e-9
-            return float(len(self.times) / total_time_s) if total_time_s > 0 else 0
+            total_time_s = np.sum(self.times) * 1e-9
+            return float(self._n_scans / total_time_s) if total_time_s > 0 else 0
 
         fps = _get_fps()
         avg_fps = int(np.floor(fps))