feat(transformations): support custom reference timestamp in motion (de)compensation#1
feat(transformations): support custom reference timestamp in motion (de)compensation#1janickm wants to merge 1 commit into
Conversation
…de)compensation Add an optional `reference_timestamp_us` parameter to `MotionCompensator.motion_compensate_points` and `motion_decompensate_points`. It defaults to `frame_end_timestamp_us`, preserving the existing KITTI/nuScenes end-of-frame behavior exactly (verified: the default path is unchanged and the existing idempotence test still passes). Datasets that motion-compensate to a different reference can now express that directly. Argoverse 2 compensates each sweep to the sweep reference timestamp, which is the *start* of the sweep (offset_ns runs forward from it), not the end. Previously the AV2 converter needed a bespoke `_decompensate_to_pointtime` helper because the API hard-coded the end-of-frame reference; using the end reference applied the full intra-sweep ego motion (~1 m) as an error. The decompensation is reimplemented to evaluate the per-point sensor pose relative to the reference sensor pose via the pose graph (`inv(T_sensor_world[pt]) @ T_sensor_world[ref]`) instead of linearly interpolating the relative end->start transform; this generalizes cleanly to any reference and is at least as accurate (it uses the real trajectory at each point time). The AV2 converter now calls `motion_decompensate_points(..., reference_timestamp_us=sweep_ts)` and the local helper is removed. Adds tests: a custom-reference compensate/decompensate round-trip and a check that an explicit `reference_timestamp_us=frame_end` matches the default.
| def motion_decompensate_points( | ||
| self, | ||
| sensor_id: str, | ||
| xyz_sensorend: np.ndarray, |
There was a problem hiding this comment.
this should be named xyz_reference or maybe even better xyz_reftime
| 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] |
There was a problem hiding this comment.
sensorend is now not correct anymore - please rename as needed - we should likely add the reference timestamp to this struct unconditionally
| timestamp_unique, dtype=np.float32 | ||
| ) | ||
| T_world_sensor_pointtime_unique = self._pose_graph.evaluate_poses("world", sensor_id, timestamp_unique) | ||
| T_sensor_pointtime_sensor_ref_unique = (T_world_sensor_pointtime_unique @ T_sensor_world_ref).astype(np.float32) |
There was a problem hiding this comment.
the frame names in the T_from_to convention are a bit off - can you please check and refine appropriately so that everything is fully consistent?
| # Relative pose mapping the reference-time sensor frame to each point's own | ||
| # sensor frame. Point at its own timestamp must remain fixed, so we evaluate | ||
| # the per-point sensor pose relative to the reference sensor pose: | ||
| # T_sensorPointtime_sensorRef = inv(T_sensor_world[pt]) @ T_sensor_world[ref] |
There was a problem hiding this comment.
while it groups things better, using camera-case is not consistent here - let's stick to T_<source-frame-name>[_time]_<target-frame-name>[_time][_suffix] (with suffix=unique or so)
| # Lidar | ||
| # ------------------------------------------------------------------------- | ||
|
|
||
| @staticmethod |
There was a problem hiding this comment.
let's make this free of the argoverse stuff - we put this on top afterwards
|
|
||
| @dataclass | ||
| class MotionCompensationResult: | ||
| xyz_s_sensorend: ( |
There was a problem hiding this comment.
please check everywhere for stray "sensorend" (both in code + comments) that need to be updated now
|
Superseded: re-created against NVIDIA:main as a standalone branch (off main, no Argoverse 2 changes), with the review feedback addressed (xyz_reftime naming, reference_timestamp_us on the result struct, snake_case T_ convention, stray sensorend cleanup). |
1 participant
Summary
Add an optional
reference_timestamp_usparameter toMotionCompensator.motion_compensate_pointsandmotion_decompensate_points, so callers can choose which in-frame timestamp the points are motion-compensated to.It defaults to
frame_end_timestamp_us, preserving the existing end-of-frame behavior (KITTI / nuScenes) exactly -- the default code path is unchanged and the existing idempotence test still passes.Motivation
Argoverse 2 motion-compensates each lidar sweep to the sweep reference timestamp, which is the start of the sweep (
offset_nsruns forward from it), not the end of the frame. The previous API hard-coded the end-of-frame reference, so the AV2 converter (NVIDIA#145) needed a bespoke_decompensate_to_pointtimehelper -- using the end reference applied the full intra-sweep ego motion (~1 m) as a systematic error to every point.This generalizes the compensator so the convention is an explicit parameter, and removes the converter-local helper.
Changes
motion_compensate_points/motion_decompensate_points: newreference_timestamp_us: Optional[int] = None(defaults toframe_end_timestamp_us); asserted to lie within[frame_start, frame_end].motion_decompensate_pointsis reimplemented to evaluate the per-point sensor pose relative to the reference sensor pose via the pose graph:instead of linearly interpolating the end->start relative transform. This generalizes to any reference and is at least as accurate (it uses the real trajectory at each point time rather than a two-point linear approximation of the relative transform).
AV2 converter now calls
motion_decompensate_points(..., reference_timestamp_us=sweep_ts); the local_decompensate_to_pointtimehelper is deleted.Testing
New tests:
test_idempotence_custom_reference_timestamp-- compensate to a non-default reference (frame start) and decompensate back recovers the original points.test_decompensate_default_reference_matches_explicit_end--reference_timestamp_us=frame_endequals the default (None).The existing
test_idempotence(default end-of-frame reference) still passes, and the AV2 integration test's lidar/cuboid alignment guard passes with the converter switched to the generalized API.