Move into odometry

PRBonn · Jan 16, 2025 · 1c31fb5 · 1c31fb5
1 parent daa72bf
commit 1c31fb5
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 20 deletions.
diff --git a/src/mapmos/odometry.py b/src/mapmos/odometry.py
@@ -60,10 +60,10 @@ def __init__(
 
     def register_points(self, points, timestamps, scan_index):
         # Apply motion compensation
-        points = self.preprocessor.preprocess(points, timestamps, self.last_delta)
+        points_prep = self.preprocessor.preprocess(points, timestamps, self.last_delta)
 
         # Voxelize
-        source, points_downsample = self.voxelize(points)
+        source, points_downsample = self.voxelize(points_prep)
 
         # Get motion prediction and adaptive_threshold
         sigma = self.adaptive_threshold.get_threshold()
@@ -79,6 +79,8 @@ 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
 
@@ -88,19 +90,26 @@ 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)
+
     def get_map_points(self):
         map_points, map_timestamps = self.local_map.point_cloud_with_timestamps()
         return map_points.reshape(-1, 3), map_timestamps.reshape(-1, 1)
 
     def current_location(self):
         return self.last_pose[:3, 3]
 
+    def transform(self, points, pose):
+        points_hom = np.hstack((points, np.ones((len(points), 1))))
+        points = (pose @ points_hom.T).T[:, :3]
+        return points
+
     def deskew(self, points, timestamps, relative_motion):
         return (
             np.asarray(
                 mapmos_pybind._deskew(
                     frame=mapmos_pybind._Vector3dVector(points),
-                    timestamps=timestamps.ravel(),
+                    timestamps=timestamps,
                     relative_motion=relative_motion,
                 )
             )

diff --git a/src/mapmos/paper_pipeline.py b/src/mapmos/paper_pipeline.py
@@ -20,7 +20,6 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 
-import copy
 import time
 from pathlib import Path
 from typing import Optional
@@ -74,13 +73,9 @@ def _run_pipeline(self):
             local_scan, timestamps, gt_labels = self._next(scan_index)
             map_points, map_indices = self.odometry.get_map_points()
 
-            last_delta = copy.copy(self.odometry.last_delta)
-            self.odometry.register_points(local_scan, timestamps, scan_index)
+            scan_points = self.odometry.register_points(local_scan, timestamps, scan_index)
             self.poses[scan_index - self._first] = self.odometry.last_pose
 
-            scan_points = self.odometry.deskew(local_scan, timestamps, last_delta)
-            scan_points = self._transform(scan_points, self.odometry.last_pose)
-
             min_range_mos = self.config.mos.min_range_mos
             max_range_mos = self.config.mos.max_range_mos
             scan_mask = self._preprocess(scan_points, min_range_mos, max_range_mos)

diff --git a/src/mapmos/pipeline.py b/src/mapmos/pipeline.py
@@ -20,7 +20,6 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 
-import copy
 import os
 import time
 from collections import deque
@@ -121,25 +120,16 @@ def _preprocess(self, points, min_range, max_range):
         mask = np.logical_and(mask, ranges >= min_range)
         return mask
 
-    def _transform(self, points, pose):
-        points_hom = np.hstack((points, np.ones((len(points), 1))))
-        points = (pose @ points_hom.T).T[:, :3]
-        return points
-
     # Private interface  ------
     def _run_pipeline(self):
         pbar = trange(self._first, self._last, unit=" frames", dynamic_ncols=True)
         for scan_index in pbar:
             local_scan, timestamps, gt_labels = self._next(scan_index)
             map_points, map_indices = self.odometry.get_map_points()
 
-            last_delta = copy.copy(self.odometry.last_delta)
-            self.odometry.register_points(local_scan, timestamps, scan_index)
+            scan_points = self.odometry.register_points(local_scan, timestamps, scan_index)
             self.poses[scan_index - self._first] = self.odometry.last_pose
 
-            scan_points = self.odometry.deskew(local_scan, timestamps, last_delta)
-            scan_points = self._transform(scan_points, self.odometry.last_pose)
-
             min_range_mos = self.config.mos.min_range_mos
             max_range_mos = self.config.mos.max_range_mos
             scan_mask = self._preprocess(scan_points, min_range_mos, max_range_mos)