feat(transformations): support custom reference timestamp in motion (de)compensation

ncore/impl/common/transformations.py

@@ -566,10 +566,12 @@ def __init__(
 
     @dataclass
     class MotionCompensationResult:
-        xyz_s_sensorend: (
-            np.ndarray
-        )  # motion-compensated ray segment start points, relative to *sensor end frame*, [N,3]
-        xyz_e_sensorend: np.ndarray  # motion-compensated ray segment end points , relative to *sensor end frame* [N,3]
+        # motion-compensated ray segment start points, relative to the sensor frame at the reference time, [N,3]
+        xyz_s_reftime: np.ndarray
+        # motion-compensated ray segment end points, relative to the sensor frame at the reference time, [N,3]
+        xyz_e_reftime: np.ndarray
+        # timestamp of the sensor reference frame the points are expressed in
+        reference_timestamp_us: int
 
     def motion_compensate_points(
         self,
@@ -578,109 +580,145 @@ def motion_compensate_points(
         timestamp_us: np.ndarray,
         frame_start_timestamp_us: int,
         frame_end_timestamp_us: int,
+        reference_timestamp_us: Optional[int] = None,
     ) -> MotionCompensationResult:
         """
-        Perform motion compensation of points in time-dependent sensor frame at measurement time to common *end-of-frame* sensor frame
+        Perform motion compensation of points in time-dependent sensor frame at measurement time to a common reference sensor frame
+
+        By default the reference is the end of the frame (``frame_end_timestamp_us``).
+        Pass ``reference_timestamp_us`` to compensate to a different reference frame
+        (e.g. the start of the frame). The chosen reference
+        must be inverted by :meth:`motion_decompensate_points` using the same
+        ``reference_timestamp_us``.
 
         Args:
             sensor_id (str): sensor the points are relative to
             xyz_pointtime(np.ndarray): points in time-dependent sensor frame (~before motion compensation), [N,3]
             timestamp_us(np.ndarray): timestamps of points, [N]
-            frame_start_timestamp_us(list): frame start timestamp, [2]
-            frame_end_timestamp_us(list): frame end timestamps, [2]
+            frame_start_timestamp_us(int): frame start timestamp
+            frame_end_timestamp_us(int): frame end timestamp
+            reference_timestamp_us(Optional[int]): timestamp of the reference sensor frame to
+                compensate to; defaults to ``frame_end_timestamp_us``. Must lie within
+                ``[frame_start_timestamp_us, frame_end_timestamp_us]``.
         Returns:
             MotionCompensationResult: result of the motion-compensation
         """
 
         # Sanity check timestamp consistency
         assert len(xyz_pointtime) == len(timestamp_us)
 
+        if reference_timestamp_us is None:
+            reference_timestamp_us = frame_end_timestamp_us
+
         if not len(xyz_pointtime):
             return self.MotionCompensationResult(
-                np.empty_like(xyz_pointtime, shape=(0, 3)), np.empty_like(xyz_pointtime, shape=(0, 3))
+                np.empty_like(xyz_pointtime, shape=(0, 3)),
+                np.empty_like(xyz_pointtime, shape=(0, 3)),
+                reference_timestamp_us,
             )
 
         assert frame_start_timestamp_us <= timestamp_us.min() and timestamp_us.max() <= frame_end_timestamp_us, (
             "Point timestamps not in frame time bounds"
         )
+        assert frame_start_timestamp_us <= reference_timestamp_us <= frame_end_timestamp_us, (
+            "Reference timestamp not in frame time bounds"
+        )
 
-        # Interpolate egomotion at frame end timestamp for sensor reference pose at end-of-frame time
-        T_world_sensorRef = self._pose_graph.evaluate_poses(
-            "world", sensor_id, np.array(frame_end_timestamp_us, dtype=np.uint64)
+        # Interpolate egomotion at the reference timestamp for the reference sensor pose
+        T_world_sensor_reftime = self._pose_graph.evaluate_poses(
+            "world", sensor_id, np.array(reference_timestamp_us, dtype=np.uint64)
         )
 
         # Determine unique timestamps to only perform actually required pose interpolations (a lot of points share the same timestamp)
         timestamp_unique, unique_timestamp_reverse_idxs = np.unique(timestamp_us, return_inverse=True)
 
-        # Frame poses for each point (will throw in case invalid timestamps are loaded) expressed in the reference sensor's frame
-        T_sensor_sensorRef_unique = (
-            T_world_sensorRef @ self._pose_graph.evaluate_poses(sensor_id, "world", timestamp_unique)
+        # Per-point transform mapping the point-time sensor frame to the reference-time
+        # sensor frame:
+        #   T_sensor_pointtime_sensor_reftime = T_world_sensor_reftime @ T_sensor_pointtime_world
+        T_sensor_pointtime_sensor_reftime_unique = (
+            T_world_sensor_reftime @ self._pose_graph.evaluate_poses(sensor_id, "world", timestamp_unique)
         ).astype(np.float32)
 
-        # Pick sensor positions (in end-of-frame reference pose) for each start point (blow up to original potentially non-unique timestamp range)
-        xyz_s_sensorend = T_sensor_sensorRef_unique[unique_timestamp_reverse_idxs, :3, -1]  # N x 3
+        # Pick sensor positions (in the reference-time pose) for each start point (blow up to original potentially non-unique timestamp range)
+        xyz_s_reftime = T_sensor_pointtime_sensor_reftime_unique[unique_timestamp_reverse_idxs, :3, -1]  # N x 3
 
-        # Apply time-dependent transforamtions to all points
-        xyz_e_sensorend = transform_point_cloud(
-            xyz_pointtime[:, np.newaxis, :], T_sensor_sensorRef_unique[unique_timestamp_reverse_idxs]
+        # Apply time-dependent transformations to all points
+        xyz_e_reftime = transform_point_cloud(
+            xyz_pointtime[:, np.newaxis, :], T_sensor_pointtime_sensor_reftime_unique[unique_timestamp_reverse_idxs]
         ).squeeze(1)  # N x 3
 
-        return self.MotionCompensationResult(xyz_s_sensorend, xyz_e_sensorend)
+        return self.MotionCompensationResult(xyz_s_reftime, xyz_e_reftime, reference_timestamp_us)
 
     def motion_decompensate_points(
         self,
         sensor_id: str,
-        xyz_sensorend: np.ndarray,
+        xyz_reftime: np.ndarray,
         timestamp_us: np.ndarray,
         frame_start_timestamp_us: int,
         frame_end_timestamp_us: int,
+        reference_timestamp_us: Optional[int] = None,
     ) -> np.ndarray:
         """
-        Decompensate motion of motin-compensated ponts to bring points into time-dependent sensor-frame
+        Decompensate motion of motion-compensated points to bring points into time-dependent sensor-frame
+
+        The input points are assumed to be expressed in the sensor frame at a single
+        *reference* timestamp (the timestamp the data was motion-compensated to). By
+        default this reference is the end of the frame (``frame_end_timestamp_us``),
+        matching :meth:`motion_compensate_points`. Datasets that compensate to a
+        different reference (e.g. the start of the frame) can pass
+        ``reference_timestamp_us`` explicitly.
 
         Args:
             sensor_id (str): sensor the points are relative to
-            xyz_sensorend (np.array): motion-compensated points relative to sensor-end-frame to be decompensated, [N,3]
+            xyz_reftime (np.array): motion-compensated points relative to the reference-time sensor frame to be decompensated, [N,3]
             timestamp_us(np.ndarray): timestamps of points, [N]
-            frame_start_timestamp_us(list): frame start timestamp, [2]
-            frame_end_timestamp_us(list): frame end timestamps, [2]
+            frame_start_timestamp_us(int): frame start timestamp
+            frame_end_timestamp_us(int): frame end timestamp
+            reference_timestamp_us(Optional[int]): timestamp of the sensor frame the points are
+                expressed in; defaults to ``frame_end_timestamp_us``. Must lie within
+                ``[frame_start_timestamp_us, frame_end_timestamp_us]``.
         Returns:
             xyz_pointtime(np.array): points in time-dependent sensor frame after motion-decompensation [n,3]
         """
 
         # Sanity check timestamp consistency
-        assert len(xyz_sensorend) == len(timestamp_us)
+        assert len(xyz_reftime) == len(timestamp_us)
 
-        if not len(xyz_sensorend):
-            return np.empty_like(xyz_sensorend, shape=(0, 3))
+        if not len(xyz_reftime):
+            return np.empty_like(xyz_reftime, shape=(0, 3))
+
+        if reference_timestamp_us is None:
+            reference_timestamp_us = frame_end_timestamp_us
 
         assert frame_start_timestamp_us <= timestamp_us.min() and timestamp_us.max() <= frame_end_timestamp_us, (
             "Point timestamps not in frame time bounds"
         )
-
-        # Construct relative pose from end-of-frame reference coordinate system to start-of-frame coordinate system
-        T_sensor_worlds = self._pose_graph.evaluate_poses(
-            sensor_id, "world", np.array([frame_start_timestamp_us, frame_end_timestamp_us], dtype=np.uint64)
+        assert frame_start_timestamp_us <= reference_timestamp_us <= frame_end_timestamp_us, (
+            "Reference timestamp not in frame time bounds"
         )
 
-        T_sensor_end_sensor_start = (se3_inverse(T_sensor_worlds[0]) @ T_sensor_worlds[1]).astype(np.float32)
-
-        relative_frame_interpolator = PoseInterpolator(
-            np.stack([T_sensor_end_sensor_start, np.eye(4, dtype=np.float32)]),
-            [frame_start_timestamp_us, frame_end_timestamp_us],
-        )
+        # Decompensation maps points from the reference-time sensor frame to each
+        # point's own sensor frame. This is the exact inverse of
+        # :meth:`motion_compensate_points`: per point we build the transform from the
+        # reference-time sensor frame to the point-time sensor frame:
+        #   T_sensor_reftime_sensor_pointtime = T_world_sensor_pointtime @ T_sensor_reftime_world
+        # A point at the reference timestamp maps through the identity, as expected.
+        T_sensor_reftime_world = self._pose_graph.evaluate_poses(
+            sensor_id, "world", np.array(reference_timestamp_us, dtype=np.uint64)
+        )  # [4, 4]
 
         # Determine unique timestamps to only perform actually required pose interpolations (a lot of points share the same timestamp)
         timestamp_unique, unique_timestamp_reverse_idxs = np.unique(timestamp_us, return_inverse=True)
 
-        # Interpolate the decompensation transformations
-        T_sensor_end_sensor_pointtime_unique = relative_frame_interpolator.interpolate_to_timestamps(
-            timestamp_unique, dtype=np.float32
+        # evaluate_poses(world, sensor) yields T_world_sensor_pointtime (the inverse of T_sensor_pointtime_world).
+        T_world_sensor_pointtime_unique = self._pose_graph.evaluate_poses("world", sensor_id, timestamp_unique)
+        T_sensor_reftime_sensor_pointtime_unique = (T_world_sensor_pointtime_unique @ T_sensor_reftime_world).astype(
+            np.float32
         )
 
         # Apply the decompensation transformations
         xyz_pointtime = transform_point_cloud(
-            xyz_sensorend[:, np.newaxis, :], T_sensor_end_sensor_pointtime_unique[unique_timestamp_reverse_idxs]
+            xyz_reftime[:, np.newaxis, :], T_sensor_reftime_sensor_pointtime_unique[unique_timestamp_reverse_idxs]
         ).squeeze(1)
 
         return xyz_pointtime

ncore/impl/common/transformations_test.py

@@ -238,15 +238,15 @@ def test_idempotence(self):
             # Re-run decompensation on compensated points
             xyz_m = motion_compensator.motion_decompensate_points(
                 lidar_sensor.sensor_id,
-                motion_compensation_result.xyz_e_sensorend,
+                motion_compensation_result.xyz_e_reftime,
                 timestamp_us,
                 frame_start_timestamps_us,
                 frame_end_timestamps_us,
             )
 
             xyz_s_m = motion_compensator.motion_decompensate_points(
                 lidar_sensor.sensor_id,
-                motion_compensation_result.xyz_s_sensorend,
+                motion_compensation_result.xyz_s_reftime,
                 timestamp_us,
                 frame_start_timestamps_us,
                 frame_end_timestamps_us,
@@ -275,6 +275,86 @@ def test_idempotence(self):
                 f"inconsistent end points, frame_idx {frame_idx}",
             )
 
+    def test_idempotence_custom_reference_timestamp(self):
+        """compensate/decompensate round-trip with non-default reference timestamps.
+
+        Compensating to a custom reference and decompensating back with the same
+        reference must recover the original points. Several references spanning the
+        [frame_start, frame_end] range are exercised (including the boundaries and
+        a few interior timestamps).
+        """
+        motion_compensator = MotionCompensator(self.loader.pose_graph)
+        lidar_sensor = self.loader.get_lidar_sensor("lidar_gt_top_p128_v4p5")
+
+        for frame_idx in range(0, 2):
+            xyz_m_ref = lidar_sensor.get_frame_point_cloud(
+                frame_idx, motion_compensation=False, with_start_points=False, return_index=0
+            ).xyz_m_end
+            timestamp_us = lidar_sensor.get_frame_ray_bundle_timestamp_us(frame_idx)
+            frame_start_us = lidar_sensor.get_frame_timestamp_us(frame_idx, types.FrameTimepoint.START)
+            frame_end_us = lidar_sensor.get_frame_timestamp_us(frame_idx, types.FrameTimepoint.END)
+
+            # Exercise references across the [start, end] range: both boundaries plus
+            # several interior fractions.
+            reference_candidates = {
+                int(round(frame_start_us + fraction * (frame_end_us - frame_start_us)))
+                for fraction in (0.0, 0.1, 0.25, 0.5, 0.75, 0.9, 1.0)
+            }
+
+            for reference_us in sorted(reference_candidates):
+                compensated = motion_compensator.motion_compensate_points(
+                    lidar_sensor.sensor_id,
+                    xyz_m_ref,
+                    timestamp_us,
+                    frame_start_us,
+                    frame_end_us,
+                    reference_timestamp_us=reference_us,
+                )
+                self.assertEqual(compensated.reference_timestamp_us, reference_us)
+
+                xyz_roundtrip = motion_compensator.motion_decompensate_points(
+                    lidar_sensor.sensor_id,
+                    compensated.xyz_e_reftime,
+                    timestamp_us,
+                    frame_start_us,
+                    frame_end_us,
+                    reference_timestamp_us=reference_us,
+                )
+
+                self.assertIsNone(
+                    np.testing.assert_array_almost_equal(
+                        np.zeros_like(delta := np.linalg.norm(xyz_roundtrip - xyz_m_ref, axis=1)),
+                        delta,
+                        decimal=2,
+                    ),
+                    f"custom-reference round-trip inconsistent, frame_idx {frame_idx}, reference_us {reference_us}",
+                )
+
+    def test_default_reference_matches_explicit_end_of_frame(self):
+        """Omitting reference_timestamp_us equals passing frame_end explicitly."""
+        motion_compensator = MotionCompensator(self.loader.pose_graph)
+        lidar_sensor = self.loader.get_lidar_sensor("lidar_gt_top_p128_v4p5")
+
+        xyz_m_ref = lidar_sensor.get_frame_point_cloud(
+            0, motion_compensation=False, with_start_points=False, return_index=0
+        ).xyz_m_end
+        timestamp_us = lidar_sensor.get_frame_ray_bundle_timestamp_us(0)
+        frame_start_us = lidar_sensor.get_frame_timestamp_us(0, types.FrameTimepoint.START)
+        frame_end_us = lidar_sensor.get_frame_timestamp_us(0, types.FrameTimepoint.END)
+
+        default = motion_compensator.motion_decompensate_points(
+            lidar_sensor.sensor_id, xyz_m_ref, timestamp_us, frame_start_us, frame_end_us
+        )
+        explicit = motion_compensator.motion_decompensate_points(
+            lidar_sensor.sensor_id,
+            xyz_m_ref,
+            timestamp_us,
+            frame_start_us,
+            frame_end_us,
+            reference_timestamp_us=frame_end_us,
+        )
+        np.testing.assert_array_equal(default, explicit)
+
 
 def get_SE3(t: np.ndarray) -> np.ndarray:
     """SE3 matrix with variable translation part"""

ncore/impl/data/v4/compat.py

@@ -424,8 +424,8 @@ def get_frame_point_cloud(
             )
             return RayBundleSensorProtocol.FramePointCloud(
                 motion_compensation=True,
-                xyz_m_start=motion_compensation_result.xyz_s_sensorend if with_start_points else None,
-                xyz_m_end=motion_compensation_result.xyz_e_sensorend,
+                xyz_m_start=motion_compensation_result.xyz_s_reftime if with_start_points else None,
+                xyz_m_end=motion_compensation_result.xyz_e_reftime,
             )
 
         @override

tools/data_converter/nuscenes/converter.py

@@ -502,7 +502,7 @@ def _decode_lidars(
 
                 xyz_decomp_j = motion_compensator.motion_decompensate_points(
                     sensor_id=LIDAR_ID,
-                    xyz_sensorend=xyz_j,
+                    xyz_reftime=xyz_j,
                     timestamp_us=ts_j,
                     frame_start_timestamp_us=frame_start_j,
                     frame_end_timestamp_us=frame_end_j,

tools/data_converter/structured_lidar_model.py

@@ -667,7 +667,7 @@ def align_frame(
         # Step 5: Decompensate
         xyz_decomp_full = motion_compensator.motion_decompensate_points(
             sensor_id=sensor_id,
-            xyz_sensorend=xyz_mc,
+            xyz_reftime=xyz_mc,
             timestamp_us=point_timestamps,
             frame_start_timestamp_us=frame_start_us,
             frame_end_timestamp_us=frame_end_us,
@@ -692,7 +692,7 @@ def align_frame(
     final_timestamps = compute_frame_timestamps(model_col.astype(np.int64), n_model_cols, frame_start_us, frame_end_us)
     xyz_decompensated = motion_compensator.motion_decompensate_points(
         sensor_id=sensor_id,
-        xyz_sensorend=xyz_mc[valid_mask],
+        xyz_reftime=xyz_mc[valid_mask],
         timestamp_us=final_timestamps,
         frame_start_timestamp_us=frame_start_us,
         frame_end_timestamp_us=frame_end_us,

tools/data_converter/waymo/converter.py

@@ -643,7 +643,7 @@ class RawFrameLabel3:
                 # Undo motion-compensation for ray bundle direction and distance computation
                 xyz_m = motion_compensator.motion_decompensate_points(
                     sensor_id=lidar_ncore_id,
-                    xyz_sensorend=xyz_e,
+                    xyz_reftime=xyz_e,
                     timestamp_us=point_timestamps_us,
                     frame_start_timestamp_us=frame_start_timestamp_us,
                     frame_end_timestamp_us=frame_end_timestamp_us,

tools/ncore_evaluate_lidar_model.py

@@ -387,14 +387,14 @@ def _render_frame_comparison(
             timestamp_us=timestamps,
             frame_start_timestamp_us=frame_start_us,
             frame_end_timestamp_us=frame_end_us,
-        ).xyz_e_sensorend
+        ).xyz_e_reftime
         model_mc = motion_compensator.motion_compensate_points(
             sensor_id=source_id,
             xyz_pointtime=model_sensor_pts,
             timestamp_us=timestamps,
             frame_start_timestamp_us=frame_start_us,
             frame_end_timestamp_us=frame_end_us,
-        ).xyz_e_sensorend
+        ).xyz_e_reftime
     else:
         native_mc = native_sensor_pts
         model_mc = model_sensor_pts

tools/ncore_project_pc_to_img.py

@@ -339,7 +339,7 @@ def run(params: CLIBaseParams, loader: SequenceLoaderProtocol) -> None:
                 timestamp_us=ray_sensor.get_frame_ray_bundle_timestamp_us(pc_frame_index),
                 frame_start_timestamp_us=ray_sensor.get_frame_timestamp_us(pc_frame_index, types.FrameTimepoint.START),
                 frame_end_timestamp_us=ray_sensor.get_frame_timestamp_us(pc_frame_index, types.FrameTimepoint.END),
-            ).xyz_e_sensorend
+            ).xyz_e_reftime
 
             # Transform the point cloud to the world coordinate frame
             pc_xyz = transform_point_cloud(