"""
Sensor editing for scene manipulation.
The :class:`SensorEditor` provides methods to edit individual sensors
(cameras, LiDARs, radars) including their extrinsics, intrinsics,
and world poses.
Example
-------
>>> # Access sensors and modify properties
>>> scene.camera("FRONT").shift(up=0.05)
>>> scene.lidar(0).set_lidar_intrinsics(max_range=150)
>>> # Method chaining
>>> scene.camera("FRONT") \\
... .shift(up=0.05) \\
... .rotate(pitch=-1) \\
... .scale_camera_resolution(scale=2.0)
"""
from __future__ import annotations
from typing import Literal, Optional, Iterable
import numpy as np
from scipy.spatial.transform import Rotation
from mimic_data_api.scene import Scene
from mimic_data_api.camera import Camera
from mimic_data_api.lidar import Lidar, VerticalResolution
from mimic_data_api.radar import Radar
from mimic_scene_edition.editors.base import log_method, check_value_limits, EditionDict
from mimic_scene_edition.core.geometry import euler_xyz_deg, make_T, se3_resample
from mimic_data_api.trajectory_numpy import WorldTrajectory
Frame = Literal["vehicle", "sensor"]
"""Frame for extrinsic operations: ``"vehicle"`` or ``"sensor"``."""
Convention = Literal["vehicle", "opencv"]
"""Coordinate convention: ``"vehicle"`` (x-forward) or ``"opencv"`` (z-forward)."""
[docs]
class SensorEditor:
"""Editor for manipulating individual sensors.
Provides methods to modify sensor extrinsics (mounting position/orientation),
intrinsics (sensor-specific parameters), and world poses. Supports cameras,
LiDARs, and radars.
Parameters
----------
scene : Scene
The scene containing the sensor.
sensor : Camera | Lidar | Radar
The sensor object to edit.
limited_mode : bool, optional
If True, enable parameter value limit checking. Default is False.
value_limits : dict[str, tuple[float, float]] | None, optional
Dictionary mapping parameter names to (min, max) bounds.
editions : list[EditionDict] | None, optional
List to append edition records to.
Attributes
----------
sensor : Camera | Lidar | Radar
The underlying sensor object being edited.
extrinsics : NDArray[4,4]
The sensor-to-vehicle transformation matrix.
intrinsics : varies
Sensor-specific intrinsic parameters.
Notes
-----
SensorEditor instances should be obtained through ``SceneEditor.camera(ref)``,
``SceneEditor.lidar(ref)``, or ``SceneEditor.radar(ref)`` rather than
instantiated directly.
All transformation methods return ``self`` to enable method chaining.
Examples
--------
Camera adjustments:
>>> scene.camera("FRONT").shift(up=0.05) # Raise camera 5cm
>>> scene.camera("FRONT").rotate(pitch=-2) # Tilt down 2°
LiDAR configuration:
>>> scene.lidar("LIDAR_TOP").set_lidar_intrinsics(
... max_range=150.0,
... min_range=1.0
... )
Method chaining:
>>> scene.camera("FRONT") \\
... .shift(up=0.05) \\
... .rotate(pitch=-1) \\
... .scale_camera_resolution(scale=2.0)
"""
[docs]
def __init__(
self,
*,
scene: Scene,
sensor: Camera | Lidar | Radar,
limited_mode: bool = False,
value_limits: dict[str, tuple[float, float]] | None = None,
editions: list[EditionDict] | None = None,
):
self.scene = scene
self.sensor = sensor
self.limited_mode = bool(limited_mode)
self.value_limits = (value_limits or {}).copy()
self._editions = editions if editions is not None else []
# ---------- Properties ----------
@property
def extrinsics(self):
"""The sensor-to-vehicle transformation matrix.
Returns
-------
NDArray[4, 4]
4x4 homogeneous transformation matrix from sensor
frame to vehicle frame.
"""
return self.sensor.extrinsics
@property
def intrinsics(self):
"""The sensor's intrinsic parameters.
Returns
-------
varies
Sensor-specific intrinsic parameters object.
Type depends on sensor type (CameraIntrinsics,
LidarIntrinsics, or RadarIntrinsics).
"""
return self.sensor.intrinsics
# ---------- Utils ----------
def _maybe_update_world_poses(
self,
recompute: bool,
previous_extrinsics: np.ndarray,
) -> "SensorEditor":
"""Optionally update world poses after extrinsic modification.
Applies the extrinsic delta to the existing world poses so pose
corrections such as bundle adjustment are preserved.
Parameters
----------
recompute : bool
If True, update world poses.
previous_extrinsics : np.ndarray
Extrinsics before the modification.
Returns
-------
SensorEditor
Self for method chaining.
"""
if not recompute:
return self
delta = np.linalg.inv(previous_extrinsics) @ np.asarray(
self.sensor.extrinsics, float
)
poses = np.asarray(self.sensor.trajectory.object2world, float)
self.sensor.trajectory.object2world = poses @ delta
return self
[docs]
@log_method
def scale_camera_resolution(
self,
scale: float,
) -> "SensorEditor":
"""Scale the camera resolution and adjust intrinsics accordingly.
Scales the camera's image dimensions and updates intrinsic
parameters (focal length, principal point, skew) to match.
Parameters
----------
scale : float
Scale factor for resolution:
- ``0.5``: Half resolution
- ``2.0``: Double resolution
Returns
-------
SensorEditor
Self for method chaining.
Raises
------
TypeError
If the sensor is not a Camera.
Notes
-----
The following intrinsic parameters are modified:
- ``fx``, ``fy``, ``cx``, ``cy``, ``skew``: Scaled by ``scale``
- ``width``, ``height``: Scaled and rounded to integers
Examples
--------
>>> scene.camera("FRONT").scale_camera_resolution(scale=0.5) # Half resolution
>>> scene.camera("FRONT").scale_camera_resolution(scale=2.0) # Double resolution
"""
cam = self._ensure_camera()
if getattr(cam, "_scale_base", None) is None:
intr = cam.intrinsics
cam._scale_base = {
"fx": float(intr.fx),
"fy": float(intr.fy),
"cx": float(intr.cx),
"cy": float(intr.cy),
"skew": float(intr.skew),
"width": int(intr.width),
"height": int(intr.height),
}
base = cam._scale_base
s = float(scale)
cam.intrinsics.fx = base["fx"] * s
cam.intrinsics.fy = base["fy"] * s
cam.intrinsics.skew = base["skew"] * s
cam.intrinsics.cx = base["cx"] * s
cam.intrinsics.cy = base["cy"] * s
cam.intrinsics.width = int(round(base["width"] * s))
cam.intrinsics.height = int(round(base["height"] * s))
# hint for overlay cosmetics (optional)
cam._render_scale = s
return self
# ---------- Extrinsics ----------
[docs]
@log_method
@check_value_limits
def set_extrinsics(
self,
*,
translation_m: Iterable[float] = (0.0, 0.0, 0.0),
rpy_deg: Iterable[float] = (0.0, 0.0, 0.0),
convention: Convention = "vehicle",
) -> "SensorEditor":
"""Set the sensor extrinsics (sensor-to-vehicle transform) directly.
Replaces the current extrinsics with a new transformation
specified by translation and rotation.
Parameters
----------
translation_m : Iterable[float], optional
Translation as (x, y, z) in meters. Default is (0, 0, 0).
rpy_deg : Iterable[float], optional
Rotation as (roll, pitch, yaw) in degrees. Default is (0, 0, 0).
convention : {"vehicle", "opencv"}, optional
Coordinate convention for the input:
- ``"vehicle"``: x-forward, y-left, z-up (default)
- ``"opencv"``: x-right, y-down, z-forward
Returns
-------
SensorEditor
Self for method chaining.
Examples
--------
>>> scene.camera("FRONT").set_extrinsics(
... translation_m=(1.5, 0.0, 1.8),
... rpy_deg=(0, -5, 0)
... )
"""
Rv, tv = self._inputs_to_vehicle_RT(translation_m, rpy_deg, convention)
self.sensor.extrinsics = make_T(Rv, tv) # (4,4)
return self
[docs]
@log_method
@check_value_limits
def offset_extrinsics(
self,
*,
dtranslation_m: Iterable[float] = (0.0, 0.0, 0.0),
drpy_deg: Iterable[float] = (0.0, 0.0, 0.0),
frame: Frame = "vehicle",
convention: Convention = "vehicle",
recompute: bool = True,
) -> "SensorEditor":
"""Apply an incremental offset to the sensor extrinsics.
Modifies the existing extrinsics by applying an additional
transformation, either in the vehicle frame or sensor frame.
Parameters
----------
dtranslation_m : Iterable[float], optional
Incremental translation as (x, y, z) in meters. Default is (0, 0, 0).
drpy_deg : Iterable[float], optional
Incremental rotation as (roll, pitch, yaw) in degrees. Default is (0, 0, 0).
frame : {"vehicle", "sensor"}, optional
Frame in which to apply the offset:
- ``"vehicle"``: Apply in vehicle coordinates (default)
- ``"sensor"``: Apply in sensor coordinates
convention : {"vehicle", "opencv"}, optional
Coordinate convention for the input. Default is ``"vehicle"``.
recompute : bool, optional
If True (default), recompute world poses after modification.
Returns
-------
SensorEditor
Self for method chaining.
Examples
--------
>>> scene.camera("FRONT").offset_extrinsics(
... dtranslation_m=(0.1, 0.0, 0.0),
... drpy_deg=(0, -2, 0)
... )
"""
Rv, tv = self._inputs_to_vehicle_RT(dtranslation_m, drpy_deg, convention)
dT_v = make_T(Rv, tv)
E = np.asarray(self.sensor.extrinsics, float)
if frame == "vehicle":
self.sensor.extrinsics = dT_v @ E
else:
# sensor-frame increment: Δs = E^{-1} Δv E
Einv = np.linalg.inv(E)
dT_s = Einv @ dT_v @ E
self.sensor.extrinsics = E @ dT_s
return self._maybe_update_world_poses(recompute, previous_extrinsics=E)
[docs]
@log_method
@check_value_limits
def shift(self, *, recompute: bool = True, **kw) -> "SensorEditor":
"""Shift the sensor mounting position in vehicle coordinates.
Translates the sensor in the vehicle frame using intuitive
direction names. This modifies the extrinsics.
Parameters
----------
forward : float, optional
Distance forward (positive X) in meters.
backward : float, optional
Distance backward (negative X) in meters.
left : float, optional
Distance left (positive Y) in meters.
right : float, optional
Distance right (negative Y) in meters.
up : float, optional
Distance up (positive Z) in meters.
down : float, optional
Distance down (negative Z) in meters.
recompute : bool, optional
If True (default), recompute world poses after modification.
Returns
-------
SensorEditor
Self for method chaining.
Examples
--------
>>> scene.camera("FRONT").shift(up=0.05) # Raise camera 5cm
>>> scene.camera("FRONT").shift(forward=0.1) # Move forward 10cm
>>> scene.lidar("LIDAR_TOP").shift(up=0.1) # Raise LiDAR
See Also
--------
rotate : Rotate the sensor.
transform : Combined translation and rotation.
"""
convention = "vehicle"
return self.transform(
referential="vehicle", convention=convention, **kw, recompute=recompute
)
[docs]
@log_method
@check_value_limits
def rotate(
self,
*,
roll: float = 0.0,
pitch: float = 0.0,
yaw: float = 0.0,
recompute: bool = True,
) -> "SensorEditor":
"""Rotate the sensor in its local frame.
Applies a rotation to the sensor mounting orientation.
For cameras, rotation is in the sensor's native convention
(typically OpenCV).
Parameters
----------
roll : float, optional
Rotation around the X axis in degrees. Default is 0.
pitch : float, optional
Rotation around the Y axis in degrees. Default is 0.
yaw : float, optional
Rotation around the Z axis in degrees. Default is 0.
recompute : bool, optional
If True (default), recompute world poses after modification.
Returns
-------
SensorEditor
Self for method chaining.
Examples
--------
>>> scene.camera("FRONT").rotate(pitch=-2) # Tilt down 2°
>>> scene.camera("FRONT").rotate(yaw=5) # Pan right 5°
>>> scene.lidar("LIDAR_TOP").rotate(roll=1) # Slight roll correction
See Also
--------
shift : Translate the sensor.
transform : Combined translation and rotation.
"""
convention = "opencv" if isinstance(self.sensor, Camera) else "vehicle"
return self.transform(
roll=roll,
pitch=pitch,
yaw=yaw,
convention=convention,
referential="sensor",
recompute=recompute,
)
[docs]
@staticmethod
def get_rotation_from_euler(
roll: float = 0, pitch: float = 0, yaw: float = 0
) -> np.ndarray:
"""Convert Euler angles to a rotation matrix.
Parameters
----------
roll : float, optional
Rotation around X axis in degrees. Default is 0.
pitch : float, optional
Rotation around Y axis in degrees. Default is 0.
yaw : float, optional
Rotation around Z axis in degrees. Default is 0.
Returns
-------
np.ndarray
3x3 rotation matrix.
"""
return Rotation.from_euler("ZYX", [yaw, pitch, roll], degrees=True).as_matrix()
# ---------- Intrinsics ----------
[docs]
@log_method
def set_camera_intrinsics(
self,
*,
fx: Optional[float] = None,
fy: Optional[float] = None,
cx: Optional[float] = None,
cy: Optional[float] = None,
width: Optional[int] = None,
height: Optional[int] = None,
distortion: Optional[object] = None,
model: Optional[object] = None,
rolling_shutter: Optional[object] = None,
) -> "SensorEditor":
"""Modify camera intrinsic parameters.
Updates camera intrinsics. Only provided parameters are modified;
others retain their current values.
Parameters
----------
fx : float | None, optional
Focal length in X (pixels).
fy : float | None, optional
Focal length in Y (pixels).
cx : float | None, optional
Principal point X coordinate (pixels).
cy : float | None, optional
Principal point Y coordinate (pixels).
width : int | None, optional
Image width in pixels.
height : int | None, optional
Image height in pixels.
distortion : DistortionParameters | None, optional
Lens distortion parameters.
model : CameraModels | None, optional
Camera model type.
rolling_shutter : RollingShutterParameters | None, optional
Rolling shutter parameters.
Returns
-------
SensorEditor
Self for method chaining.
Raises
------
TypeError
If the sensor is not a Camera.
Examples
--------
>>> scene.camera("FRONT").set_camera_intrinsics(
... fx=1200, fy=1200,
... cx=640, cy=360,
... width=1280, height=720
... )
"""
cam = self._ensure_camera()
intr = cam.intrinsics
updates = dict(
fx=fx,
fy=fy,
cx=cx,
cy=cy,
width=width,
height=height,
distortion=distortion,
model=model,
rolling_shutter=rolling_shutter,
)
for k, v in updates.items():
if v is not None and hasattr(intr, k):
setattr(intr, k, v)
return self
[docs]
@log_method
def set_lidar_intrinsics(
self,
*,
vertical_fov: Optional[float] = None,
horizontal_fov: Optional[float] = None,
horizontal_resolution: Optional[float] = None,
vertical_resolution_rad: Optional[
VerticalResolution | dict[int, float] | float
] = None,
vertical_resolution_deg: Optional[float] = None,
keep_vfov: bool = True,
max_range: Optional[float] = None,
min_range: Optional[float] = None,
horizontal_beam_divergence: Optional[float] = None,
vertical_beam_divergence: Optional[float] = None,
) -> "SensorEditor":
"""Modify LiDAR intrinsic parameters.
Updates LiDAR intrinsics. Only provided parameters are modified.
Parameters
----------
vertical_fov : float | None, optional
Vertical field of view in radians.
horizontal_fov : float | None, optional
Horizontal field of view in radians.
horizontal_resolution : float | None, optional
Points per radian (horizontal).
vertical_resolution_rad : VerticalResolution | dict | float | None, optional
Vertical resolution specification in radians. Can be provided as:
- ``float``: Angular spacing between beams. Behavior depends on ``keep_vfov``.
- ``dict[int, float]``: Channel-to-angle mapping (replaces existing)
- ``VerticalResolution``: Full specification object (replaces existing)
vertical_resolution_deg : float | None, optional
Convenience argument. Uniform vertical resolution in degrees.
Converted to radians internally.
keep_vfov : bool, optional
Only used when ``vertical_resolution_rad`` or ``vertical_resolution_deg``
is a float and the lidar currently has ``elevation_channel_mapping``.
- ``True`` (default): Preserve existing min/max elevation bounds.
Generates new channel mapping from max to min elevation with
the specified resolution as step size.
- ``False``: Generate symmetric FOV centered at 0, using the current
``vertical_fov`` value and the specified resolution.
max_range : float | None, optional
Maximum detection range in meters.
min_range : float | None, optional
Minimum detection range in meters.
horizontal_beam_divergence : float | None, optional
Horizontal beam divergence in radians.
vertical_beam_divergence : float | None, optional
Vertical beam divergence in radians.
Returns
-------
SensorEditor
Self for method chaining.
Raises
------
TypeError
If the sensor is not a Lidar.
ValueError
If both vertical_resolution_rad and vertical_resolution_deg are specified.
Examples
--------
>>> scene.lidar("LIDAR_TOP").set_lidar_intrinsics(
... max_range=150.0,
... min_range=1.0,
... )
Change vertical resolution while preserving existing FOV bounds:
>>> scene.lidar("LIDAR_TOP").set_lidar_intrinsics(vertical_resolution_deg=0.5)
Change vertical resolution with symmetric FOV:
>>> scene.lidar("LIDAR_TOP").set_lidar_intrinsics(
... vertical_resolution_deg=0.5,
... keep_vfov=False
... )
"""
lid = self._ensure_lidar()
intr = lid.intrinsics
if vertical_resolution_deg is not None:
if vertical_resolution_rad is not None:
raise ValueError(
"Cannot specify both vertical_resolution_rad and vertical_resolution_deg"
)
vertical_resolution_rad = float(np.deg2rad(vertical_resolution_deg))
if vertical_resolution_rad is not None:
if isinstance(vertical_resolution_rad, VerticalResolution):
intr.vertical_resolution = vertical_resolution_rad
elif isinstance(vertical_resolution_rad, dict):
intr.vertical_resolution = VerticalResolution(
elevation_channel_mapping=vertical_resolution_rad
)
else:
resolution = float(vertical_resolution_rad)
existing_vr = intr.vertical_resolution
if existing_vr.elevation_channel_mapping:
# channel mapping -> generate new mapping
if keep_vfov:
# existing min/max bounds
elevations = list(
existing_vr.elevation_channel_mapping.values()
)
min_elev = min(elevations)
max_elev = max(elevations)
else:
# generate symmetric FOV from vertical_fov
half_fov = intr.vertical_fov / 2.0
max_elev = half_fov
min_elev = -half_fov
# arange with exact resolution step (from max to min, descending)
new_elevations = np.arange(
max_elev, min_elev - resolution / 2, -resolution
)
new_mapping = dict(enumerate(new_elevations.tolist()))
intr.vertical_resolution = VerticalResolution(
elevation_channel_mapping=new_mapping
)
else:
# has uniform resolution -> keep uniform
intr.vertical_resolution = VerticalResolution(
vertical_resolution=resolution
)
for k, v in dict(
vertical_fov=vertical_fov,
horizontal_fov=horizontal_fov,
horizontal_resolution=horizontal_resolution,
max_range=max_range,
min_range=min_range,
horizontal_beam_divergence=horizontal_beam_divergence,
vertical_beam_divergence=vertical_beam_divergence,
).items():
if v is not None and hasattr(intr, k):
setattr(intr, k, v)
return self
[docs]
@log_method
def set_radar_intrinsics(
self,
*,
range: Optional[float] = None,
azimuth_fov: Optional[float] = None,
elevation_fov: Optional[float] = None,
azimuth_resolution: Optional[list[tuple[float, float]]] = None,
elevation_resolution: Optional[list[tuple[float, float]]] = None,
min_speed_resolution: Optional[float] = None,
max_speed_resolution: Optional[float] = None,
) -> "SensorEditor":
"""Modify radar intrinsic parameters.
Updates radar intrinsics. Only provided parameters are modified.
Parameters
----------
range : float | None, optional
Maximum detection range in meters.
azimuth_fov : float | None, optional
Azimuth field of view in radians.
elevation_fov : float | None, optional
Elevation field of view in radians.
azimuth_resolution : list[tuple[float, float]] | None, optional
Azimuth resolution specification.
elevation_resolution : list[tuple[float, float]] | None, optional
Elevation resolution specification.
min_speed_resolution : float | None, optional
Minimum speed resolution.
max_speed_resolution : float | None, optional
Maximum speed resolution.
Returns
-------
SensorEditor
Self for method chaining.
Raises
------
TypeError
If the sensor is not a Radar.
Examples
--------
>>> scene.radar("RADAR_FRONT").set_radar_intrinsics(
... range=200.0,
... azimuth_fov=2.0, # radians
... elevation_fov=0.3
... )
"""
rad = self._ensure_radar()
intr = rad.intrinsics
for k, v in dict(
range=range,
azimuth_fov=azimuth_fov,
elevation_fov=elevation_fov,
azimuth_resolution=azimuth_resolution,
elevation_resolution=elevation_resolution,
min_speed_resolution=min_speed_resolution,
max_speed_resolution=max_speed_resolution,
).items():
if v is not None and hasattr(intr, k):
setattr(intr, k, v)
return self
# ---------- Helpers ----------
def _ensure_camera(self) -> Camera:
"""Ensure the sensor is a Camera and return it.
Returns
-------
Camera
The sensor as a Camera object.
Raises
------
TypeError
If the sensor is not a Camera.
"""
if not isinstance(self.sensor, Camera):
raise TypeError("Sensor is not a Camera")
return self.sensor
def _ensure_lidar(self) -> Lidar:
"""Ensure the sensor is a Lidar and return it.
Returns
-------
Lidar
The sensor as a Lidar object.
Raises
------
TypeError
If the sensor is not a Lidar.
"""
if not isinstance(self.sensor, Lidar):
raise TypeError("Sensor is not a Lidar")
return self.sensor
def _ensure_radar(self) -> Radar:
"""Ensure the sensor is a Radar and return it.
Returns
-------
Radar
The sensor as a Radar object.
Raises
------
TypeError
If the sensor is not a Radar.
"""
if not isinstance(self.sensor, Radar):
raise TypeError("Sensor is not a Radar")
return self.sensor
# ---------- Internals ----------
@staticmethod
def _R_camopencv_to_vehicle() -> np.ndarray:
"""Get rotation matrix from OpenCV camera to vehicle convention.
OpenCV camera axes: x-right, y-down, z-forward
Vehicle axes: x-forward, y-left, z-up
Returns
-------
np.ndarray
3x3 rotation matrix R such that v_vehicle = R @ v_cam.
"""
return np.array(
[[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]], dtype=float
)
@staticmethod
def _R_conv_vehicle_to_camopencv() -> np.ndarray:
"""Get rotation matrix from vehicle to OpenCV camera convention.
Vehicle axes: x-forward, y-left, z-up
OpenCV camera axes: x-right, y-down, z-forward
Returns
-------
np.ndarray
3x3 rotation matrix.
"""
return np.array([[-0.0, -1.0, -0.0], [-0.0, -0.0, -1.0], [1.0, 0.0, 0.0]])
def _inputs_to_vehicle_RT(
self,
translation_m: Iterable[float],
rpy_deg: Iterable[float],
convention: Convention,
) -> tuple[np.ndarray, np.ndarray]:
"""Convert user inputs to vehicle-frame rotation and translation.
Parameters
----------
translation_m : Iterable[float]
Translation as (x, y, z) in meters.
rpy_deg : Iterable[float]
Rotation as (roll, pitch, yaw) in degrees.
convention : Convention
Input coordinate convention.
Returns
-------
tuple[np.ndarray, np.ndarray]
Rotation matrix (3x3) and translation vector (3,) in vehicle frame.
"""
t = np.asarray(list(translation_m), float).reshape(3)
roll, pitch, yaw = [float(x) for x in rpy_deg]
R_in = euler_xyz_deg(roll, pitch, yaw)
if convention == "opencv" and isinstance(self.sensor, Camera):
B = self._R_camopencv_to_vehicle()
return B @ R_in @ B.T, B @ t
return R_in, t
[docs]
@log_method
def recompute_world_poses(
self, ego_traj: WorldTrajectory | None = None
) -> "SensorEditor":
"""Rebuild the sensor's world poses from ego trajectory and extrinsics.
Recomputes the sensor-to-world transformation sequence using
the current extrinsics and ego vehicle trajectory.
The computation follows:
.. math::
S2W(t) = E2W(t) \\cdot S2E
Where:
- S2W: Sensor-to-world transform
- E2W: Ego-to-world transform (interpolated to sensor timestamps)
- S2E: Sensor-to-ego (extrinsics)
Parameters
----------
ego_traj : WorldTrajectory | None, optional
Ego trajectory to use. If None, uses the scene's ego trajectory.
Returns
-------
SensorEditor
Self for method chaining.
Notes
-----
Call this after modifying extrinsics to update the sensor's
world trajectory.
Examples
--------
>>> scene.camera("FRONT").shift(up=0.05).recompute_world_poses()
"""
s = self.sensor
E2W = ego_traj or self.scene.ego_vehicle.trajectory
t_e = np.asarray(E2W.timestamps, float)
T_e = np.asarray(E2W.object2world, float)
t_s = np.asarray(s.trajectory.timestamps, float)
E2W_at_sensor = se3_resample(t_e, T_e, t_s)
S2E = np.asarray(s.extrinsics, float)
s.trajectory.object2world = E2W_at_sensor @ S2E
return self
[docs]
@log_method
def convert_convention(
self,
*,
from_convention: Convention,
to_convention: Convention = "vehicle",
recompute: bool = True,
) -> "SensorEditor":
"""Convert the sensor extrinsics between coordinate conventions.
Transforms the extrinsics from one coordinate convention to another.
This is primarily used for cameras to convert between vehicle and
OpenCV conventions.
Parameters
----------
from_convention : {"vehicle", "opencv"}
Current coordinate convention.
to_convention : {"vehicle", "opencv"}, optional
Target coordinate convention. Default is ``"vehicle"``.
recompute : bool, optional
If True (default), recompute world poses after conversion.
Returns
-------
SensorEditor
Self for method chaining.
Notes
-----
Coordinate conventions:
- **Vehicle**: X-forward, Y-left, Z-up
- **OpenCV**: X-right, Y-down, Z-forward
Examples
--------
>>> scene.camera("FRONT").convert_convention(
... from_convention="opencv",
... to_convention="vehicle"
... )
"""
if from_convention == to_convention:
return self
E = np.asarray(self.sensor.extrinsics, float)
B = self._R_camopencv_to_vehicle()
E2W0 = np.asarray(self.scene.ego_vehicle.trajectory.object2world, float)[0]
S2W0 = np.asarray(self.sensor.trajectory.object2world, float)[0]
is_S2E = np.allclose(E2W0 @ E, S2W0, atol=1e-4)
def H(R):
H = np.eye(4)
H[:3, :3] = R
return H
if from_convention == "opencv" and to_convention == "vehicle":
# S2E: domain change (right-multiply by B^T)
# E2S: codomain change (left-multiply by B)
self.sensor.extrinsics = (E @ H(B.T)) if is_S2E else (H(B) @ E)
elif from_convention == "vehicle" and to_convention == "opencv":
# S2E: domain change (right-multiply by B)
# E2S: codomain change (left-multiply by B^T)
self.sensor.extrinsics = (E @ H(B)) if is_S2E else (H(B.T) @ E)
else:
raise ValueError("Unsupported conversion")
return self._maybe_update_world_poses(recompute, previous_extrinsics=E)