Motion Model

LiDAR and camera stamps usually differ. Projecting a LiDAR point into an image from another time shifts the pixel and can leave the classifier box.

Cones are treated as static in the world; the vehicle frame moves. On each /cones callback:

1. Select nearest left and right camera frames from the deques
2. Integrate /filter/twist between the LiDAR stamp and each camera stamp → displacement $\Delta x$ and rotation
3. Convert IMU-centered twist into the LiDAR frame with imu_to_lidar (includes $\omega \times r$ at the lever arm)
4. Apply the rigid warp, then project

For a camera stamp newer than LiDAR: $p_{\text{cam}} = R^\top (p_{\text{lidar}} - \Delta x)$ (vehicle moved; pull the point back into the camera-time vehicle frame). Older camera stamps use the inverse direction in motion_model_points_3d.

Cone history uses a planar displacement helper (get_planar_displacement / propagate_cone_to_stamp) so tracks stay comparable while the car moves. That path is more developed on feature/perc/propagate_history.

Parameters

Input	Notes
/filter/twist	Integrated over the stamp gap
imu_to_lidar	Lever arm / frame change for twist
max_deque_size	How far back frames/twist are kept

lidar_pitch_angle_rad (~14°) is passed into the 3D motion call site today and unused in the function body; do not treat it as a live tuning knob until that changes.

Good at rest, wrong while moving/yawing → twist missing, empty deques, or bad imu_to_lidar. Triage: ops.