Move deskewing (#35)

Author: benemer

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()