diff --git a/python/kiss_icp/datasets/paris_luco.py b/python/kiss_icp/datasets/paris_luco.py index 816d0645..d243ccc2 100644 --- a/python/kiss_icp/datasets/paris_luco.py +++ b/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() + new_poses[:, :3, :3] = np.eye(3) return new_poses diff --git a/python/kiss_icp/metrics.py b/python/kiss_icp/metrics.py index 002079ce..cd2d6031 100644 --- a/python/kiss_icp/metrics.py +++ b/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) diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py index f8dfd857..c3cbb169 100644 --- a/python/kiss_icp/pipeline.py +++ b/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,9 +97,11 @@ 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.visualizer.update(source, keypoints, self.odometry.local_map, self.poses[-1]) + 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.odometry.last_pose + ) def _next(self, idx): """TODO: re-arrange this logic""" @@ -112,22 +114,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 +145,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 +181,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))