From 9a8aef4fc6d07d969ca8090b4560b7884f7cc670 Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Sun, 6 Apr 2025 10:10:27 +0200 Subject: [PATCH] Move deskewing --- src/mapmos/odometry.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/mapmos/odometry.py b/src/mapmos/odometry.py index 1fb2cf4..8eed56c 100644 --- a/src/mapmos/odometry.py +++ b/src/mapmos/odometry.py @@ -79,8 +79,6 @@ def register_points(self, points, timestamps, scan_index): kernel=sigma / 3, ) - point_deskewed = self.deskew(points, timestamps, self.last_delta) - # Compute the difference between the prediction and the actual estimate model_deviation = np.linalg.inv(initial_guess) @ new_pose @@ -90,7 +88,9 @@ def register_points(self, points, timestamps, scan_index): self.last_delta = np.linalg.inv(self.last_pose) @ new_pose self.last_pose = new_pose - return self.transform(point_deskewed, self.last_pose) + points_deskewed = self.deskew(points, timestamps, self.last_delta) + + return self.transform(points_deskewed, self.last_pose) def get_map_points(self): map_points, map_timestamps = self.local_map.point_cloud_with_timestamps()