Trajectory Module
=================

.. module:: mimic_data_api.trajectory_numpy

The trajectory module provides data structures for representing time-series
pose data in world coordinates.

WorldTrajectory
---------------

.. autoclass:: WorldTrajectory
   :members:
   :undoc-members:
   :show-inheritance:

   .. rubric:: Usage Example

   .. code-block:: python

       import numpy as np
       from mimic_data_api import WorldTrajectory

       # Create a 100-frame trajectory
       trajectory = WorldTrajectory(
           timestamps=np.linspace(0, 10, 100),
           object2world=np.stack([np.eye(4)] * 100),
           linear_velocity=np.zeros((100, 3)),
           angular_velocity=np.zeros((100, 3))
       )

       # Access pose at frame 50
       pose = trajectory.object2world[50]
       position = pose[:3, 3]  # Translation
       rotation = pose[:3, :3]  # Rotation matrix

Sensor World Poses
------------------

The ego vehicle trajectory stores vehicle-to-world poses. For a mounted
sensor, ``trajectory.object2world`` stores sensor-to-world poses, not the raw
ego vehicle poses. Compose each sensor pose as:

.. math::

    T_{sensor \rightarrow world}(i) =
    T_{vehicle \rightarrow world}(i) \cdot
    T_{sensor \rightarrow vehicle}

The sensor's ``extrinsics`` field is ``T_sensor_to_vehicle``. This contract
applies to cameras, LiDARs, and radars. Sensor data files themselves remain
in the sensor-local coordinate frame.

Extrinsic Calibration
---------------------

.. module:: mimic_data_api.extrinsic

.. data:: SensorExtrinsic

   Type alias for a 4x4 sensor extrinsic calibration matrix.
   
   This matrix transforms points from the sensor coordinate frame
   to the vehicle body coordinate frame:
   
   .. math::
   
       p_{vehicle} = T_{sensor \rightarrow vehicle} \cdot p_{sensor}
   
   :type: NDArray[Shape["4 x, 4 y"], float]

   .. code-block:: python

       import numpy as np
       from mimic_data_api.extrinsic import SensorExtrinsic

       # Identity transform (sensor aligned with vehicle)
       extrinsic: SensorExtrinsic = np.eye(4)

       # LiDAR mounted 1.5m above vehicle origin
       extrinsic = np.array([
           [1, 0, 0, 0],
           [0, 1, 0, 0],
           [0, 0, 1, 1.5],
           [0, 0, 0, 1]
       ])