"""
Ego vehicle editing for scene manipulation.
The :class:`EgoEditor` provides methods to edit the ego vehicle and all
attached sensors as a unified rig. Access via ``scene.ego``.
Example
-------
>>> # Transform the ego rig
>>> scene.ego.shift(forward=5)
>>> scene.ego.rotate(yaw=15)
>>> # Perform maneuvers
>>> scene.ego.smooth_lane_change(start_idx=10, duration=30, left=3.5)
>>> scene.ego.stop(at_idx=50)
"""
from copy import deepcopy
from typing import Literal, Optional
import numpy as np
from mimic_data_api.scene import Scene
from mimic_scene_edition.editors.base import log_method, check_value_limits, EditionDict
from mimic_scene_edition.editors.trajectory_editor import TrajectoryEditor
from mimic_scene_edition.editors.sensor_editor import SensorEditor
from mimic_scene_edition.core.geometry import (
extract_translation_vector,
euler_xyz_deg,
make_T,
)
from mimic_scene_edition.core.sensors import apply_world_to_rig, apply_local_to_rig
from mimic_scene_edition.core.velocity import recompute_velocities
from mimic_scene_edition.core.limits import DEFAULT_EGO_VALUE_LIMITS
from mimic_scene_edition.strategies.curves import build_lane_change_T_seq
from mimic_scene_edition.strategies.stop import build_graceful_stop_T_seq
def _roll_images(images, k):
"""Circular roll of image list by k positions.
Parameters
----------
images : list
List of images to roll.
k : int
Number of positions to roll.
Returns
-------
list
Rolled image list.
"""
if not images:
return images
k = int(k) % len(images)
return images[-k:] + images[:-k]
def _resample_images(images, old_t, new_t):
"""Pick, for each resampled frame, the image nearest to it in time.
The renderer reads ``images_data`` one entry per trajectory sample
(rolling shutter, frame count), so a resample that leaves the list at
its old length breaks the camera build.
Parameters
----------
images : list
One image per sample of the old trajectory.
old_t : np.ndarray
Timestamps of the old trajectory.
new_t : np.ndarray
Timestamps of the resampled trajectory.
Returns
-------
list
One image per sample of the resampled trajectory.
"""
old_t = np.asarray(old_t, float)
new_t = np.asarray(new_t, float)
if not images or len(images) != len(old_t) or len(old_t) < 2:
return images
right = np.clip(np.searchsorted(old_t, new_t), 1, len(old_t) - 1)
left = right - 1
nearest = np.where(new_t - old_t[left] <= old_t[right] - new_t, left, right)
return [images[i] for i in nearest]
[docs]
class EgoEditor:
"""Editor for the ego vehicle and all attached sensors.
Provides methods to transform the ego vehicle and its sensor rig
as a unified system. All sensor trajectories are updated to maintain
their relative positions when the ego is transformed.
Parameters
----------
scene : Scene
The scene containing the ego vehicle.
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.
Notes
-----
EgoEditor instances should be obtained through ``SceneEditor.ego``
rather than instantiated directly.
All transformation methods return ``self`` to enable method chaining.
Examples
--------
Basic transformations:
>>> scene.ego.shift(forward=5) # Move forward 5m
>>> scene.ego.shift(left=2, up=0.5) # Move left and up
>>> scene.ego.rotate(yaw=15) # Turn 15° left
Maneuvers:
>>> scene.ego.smooth_lane_change(start_idx=10, duration=30, left=3.5)
>>> scene.ego.evasive_maneuver(start_idx=20, duration=40, left=2)
Stopping:
>>> scene.ego.stop(at_idx=50) # Stop at frame 50
>>> scene.ego.stop(at_idx=50, duration=20) # Stop for 20 frames
"""
[docs]
def __init__(
self,
scene: Scene,
*,
limited_mode: bool = False,
value_limits: dict[str, tuple[float, float]] | None = None,
editions: list[EditionDict] | None = None,
):
self.scene = scene
self.limited_mode = limited_mode
self.value_limits = (value_limits or DEFAULT_EGO_VALUE_LIMITS).copy()
self._bak = deepcopy(scene)
self._editions = editions if editions is not None else []
# ---------- internals ----------
def _rig_lists(self):
"""Get ego vehicle and all sensor lists.
Returns
-------
tuple
(ego_vehicle, cameras, lidars, radars)
"""
ego = self.scene.ego_vehicle
s = ego.sensors
cams = s.cameras or []
lids = s.lidars or []
rads = s.radars or []
return ego, cams, lids, rads
def _assign(self, ego_new, cams_new, lids_new, rads_new, *, recompute_vel: bool):
"""Assign new trajectories to ego and sensors.
Parameters
----------
ego_new : WorldTrajectory
New ego trajectory.
cams_new : list[WorldTrajectory]
New camera trajectories.
lids_new : list[WorldTrajectory]
New lidar trajectories.
rads_new : list[WorldTrajectory]
New radar trajectories.
recompute_vel : bool
If True, recompute velocities for all trajectories.
"""
if recompute_vel:
ego_new = recompute_velocities(ego_new)
cams_new = [recompute_velocities(tr) for tr in cams_new]
lids_new = [recompute_velocities(tr) for tr in lids_new]
rads_new = [recompute_velocities(tr) for tr in rads_new]
ego, cams, lids, rads = self._rig_lists()
ego.trajectory = ego_new
for s, n in zip(cams, cams_new):
s.trajectory = n
for s, n in zip(lids, lids_new):
s.trajectory = n
for s, n in zip(rads, rads_new):
s.trajectory = n
@staticmethod
def _window(
n_total: int, start_idx: int | None, end_idx: int | None
) -> tuple[int, int]:
"""Validate and normalize frame index window bounds.
Parameters
----------
n_total : int
Total number of frames.
start_idx : int | None
Start index (None means 0).
end_idx : int | None
End index exclusive (None means n_total).
Returns
-------
tuple[int, int]
Validated (start, end) indices.
Raises
------
ValueError
If the window bounds are invalid.
"""
i0 = 0 if start_idx is None else int(start_idx)
i1 = n_total if end_idx is None else int(end_idx)
if not (0 <= i0 <= i1 <= n_total):
raise ValueError(f"Invalid index window [{i0}, {i1}) for length {n_total}.")
return i0, i1
@staticmethod
def _te(traj):
"""Create a TrajectoryEditor without velocity recomputation.
Parameters
----------
traj : WorldTrajectory
Trajectory to edit.
Returns
-------
TrajectoryEditor
New trajectory editor.
"""
return TrajectoryEditor(traj=traj, recompute_on_mutation=False)
# -------- temporal ops via TrajectoryEditor --------
[docs]
@log_method
def resample(self, *, hz: float) -> "EgoEditor":
"""Resample the ego rig trajectory to a target frame rate.
Resamples the ego vehicle and all sensor trajectories to
produce poses at a uniform temporal spacing. Each camera
frame keeps the image nearest to it in time.
Parameters
----------
hz : float
Target frame rate in Hz (frames per second).
Returns
-------
EgoEditor
Self for method chaining.
Examples
--------
>>> scene.ego.resample(hz=20) # Resample to 20 Hz
"""
ego, cams, lids, rads = self._rig_lists()
e_te = self._te(ego.trajectory).resample_to_rate(hz)
c_new = []
for s in cams:
c_te = self._te(s.trajectory).resample_to_rate(hz)
s.images_data = _resample_images(
s.images_data, s.trajectory.timestamps, c_te.traj.timestamps
)
c_new.append(c_te.traj)
l_new = [self._te(s.trajectory).resample_to_rate(hz).traj for s in lids]
r_new = [self._te(s.trajectory).resample_to_rate(hz).traj for s in rads]
self._assign(e_te.traj, c_new, l_new, r_new, recompute_vel=True)
return self
[docs]
@check_value_limits
@log_method
def offset(self, *, offset_frame: int) -> "EgoEditor":
"""Shift the timeline by a number of frames.
Performs a circular shift of all trajectories and associated
image data, wrapping around at the boundaries.
Parameters
----------
offset_frame : int
Number of frames to shift. Positive values shift forward.
Returns
-------
EgoEditor
Self for method chaining.
Examples
--------
>>> scene.ego.offset(offset_frame=5) # Shift by 5 frames
"""
ego, cams, lids, rads = self._rig_lists()
k = int(offset_frame)
e_te = self._te(ego.trajectory).offset_frames(k)
c_new = []
for s in cams:
c_te = self._te(s.trajectory).offset_frames(k)
s.images_data = _roll_images(s.images_data, k)
c_new.append(c_te.traj)
l_new = [self._te(s.trajectory).offset_frames(k).traj for s in lids]
r_new = [self._te(s.trajectory).offset_frames(k).traj for s in rads]
self._assign(e_te.traj, c_new, l_new, r_new, recompute_vel=False)
return self
[docs]
@check_value_limits
@log_method
def stop(
self,
at_idx: int,
*,
duration: Optional[int] = None,
decel: int = 10,
profile: Literal["linear", "min_jerk"] = "min_jerk",
) -> "EgoEditor":
"""Stop the ego vehicle at a specific frame.
Brings the ego vehicle and all attached sensors to a stop,
optionally with a smooth deceleration profile.
Parameters
----------
at_idx : int
Frame index to stop at. If ``decel > 0``, this is where
the deceleration ends.
duration : int | None, optional
Number of frames to stay stopped. If None, remain stopped
until the end of the trajectory.
decel : int, optional
Number of frames for gradual deceleration. Use 0 for
instant stop. Default is 10.
profile : {"linear", "min_jerk"}, optional
Deceleration profile:
- ``"linear"``: Linear velocity reduction
- ``"min_jerk"``: Smooth minimum-jerk profile (default)
Returns
-------
EgoEditor
Self for method chaining.
Examples
--------
>>> scene.ego.stop(at_idx=50) # Stop at frame 50
>>> scene.ego.stop(at_idx=50, duration=20) # Stop for 20 frames
>>> scene.ego.stop(at_idx=50, decel=15) # Gradual 15-frame decel
"""
ego, cams, lids, rads = self._rig_lists()
if decel > 0:
T_seq, i_start, i_end = build_graceful_stop_T_seq(
ego.trajectory, at_idx=int(at_idx), decel=int(decel), profile=profile
)
if T_seq.shape[0] > 0:
self.apply_T_sequence(
T_seq, start_idx=i_start, end_idx=i_end, referential="local"
)
start = int(at_idx)
end = start + int(duration) if duration is not None else None
e_te = self._te(ego.trajectory).freeze(start, end)
c_new = [self._te(s.trajectory).freeze(start, end).traj for s in cams]
l_new = [self._te(s.trajectory).freeze(start, end).traj for s in lids]
r_new = [self._te(s.trajectory).freeze(start, end).traj for s in rads]
self._assign(e_te.traj, c_new, l_new, r_new, recompute_vel=True)
i_clamp0 = int(at_idx)
i_clamp1 = None if duration is None else int(at_idx) + int(duration)
for tr in [ego.trajectory, *[s.trajectory for s in (cams + lids + rads)]]:
if getattr(tr, "linear_velocity", None) is not None:
tr.linear_velocity[i_clamp0:i_clamp1] = 0.0
if getattr(tr, "angular_velocity", None) is not None:
tr.angular_velocity[i_clamp0:i_clamp1] = 0.0
return self
[docs]
@log_method
def stop_from(self, start_idx: int) -> "EgoEditor":
"""Freeze the ego and all sensors from *start_idx* onward.
Unlike :meth:`stop`, no deceleration ramp is applied — the pose
at *start_idx* is simply replicated for every subsequent frame
and velocities are zeroed.
Parameters
----------
start_idx : int
Frame index from which to freeze.
Returns
-------
EgoEditor
Self for method chaining.
"""
ego, cams, lids, rads = self._rig_lists()
i0 = int(start_idx)
e_te = self._te(ego.trajectory).freeze(i0, None)
c_new = [self._te(s.trajectory).freeze(i0, None).traj for s in cams]
l_new = [self._te(s.trajectory).freeze(i0, None).traj for s in lids]
r_new = [self._te(s.trajectory).freeze(i0, None).traj for s in rads]
self._assign(e_te.traj, c_new, l_new, r_new, recompute_vel=True)
for tr in [ego.trajectory, *[s.trajectory for s in (cams + lids + rads)]]:
if getattr(tr, "linear_velocity", None) is not None:
tr.linear_velocity[i0:] = 0.0
if getattr(tr, "angular_velocity", None) is not None:
tr.angular_velocity[i0:] = 0.0
return self
[docs]
@check_value_limits
@log_method
def speed(self, multiplier: float) -> "EgoEditor":
"""Change the apparent speed by retiming timestamps.
Adjusts trajectory timestamps to change perceived speed
without modifying the spatial path. Affects the ego and
all attached sensors.
Parameters
----------
multiplier : float
Speed scaling factor:
- ``> 1``: Faster (e.g., 1.5 = 50% faster)
- ``< 1``: Slower (e.g., 0.5 = 50% slower)
Returns
-------
EgoEditor
Self for method chaining.
Examples
--------
>>> scene.ego.speed(multiplier=1.5) # 50% faster
>>> scene.ego.speed(multiplier=0.5) # 50% slower
"""
ego, cams, lids, rads = self._rig_lists()
e_te = self._te(ego.trajectory).scale_speed(multiplier=multiplier)
c_new = [
self._te(s.trajectory).scale_speed(multiplier=multiplier).traj for s in cams
]
l_new = [
self._te(s.trajectory).scale_speed(multiplier=multiplier).traj for s in lids
]
r_new = [
self._te(s.trajectory).scale_speed(multiplier=multiplier).traj for s in rads
]
self._assign(e_te.traj, c_new, l_new, r_new, recompute_vel=False)
return self
[docs]
@check_value_limits
@log_method
def apply_T_sequence(
self,
T: np.ndarray,
*,
start_idx: int | None = None,
end_idx: int | None = None,
referential: str = "world",
mode: str = "auto",
coverage: str = "bin",
) -> "EgoEditor":
"""Apply a sequence of 4x4 transformation matrices.
Applies SE(3) transformation matrices to the ego and all
sensors in the specified frame range.
Parameters
----------
T : np.ndarray
Transformation matrices with shape ``(4, 4)`` for a single
transform, or ``(N, 4, 4)`` for per-frame transforms.
start_idx : int | None, optional
First frame to transform (inclusive). None means 0.
end_idx : int | None, optional
Last frame to transform (exclusive). None means end.
referential : {"world", "local"}, optional
How to apply the transformation:
- ``"world"``: Left-multiply. Sensors resampled to their times.
- ``"local"``: Right-multiply on ego; conjugate onto sensors.
Default is ``"world"``.
mode : {"auto", "conjugate", "interpolate"}, optional
Mode for local referential. Default is ``"auto"``.
coverage : {"time", "bin"}, optional
Coverage mode for world referential. Default is ``"bin"``.
Returns
-------
EgoEditor
Self for method chaining.
Notes
-----
**World referential (left-multiply):**
- E2W' = T_world @ E2W
- S2W' = T_world @ S2W (resampled to each sensor time)
**Local referential (right-multiply):**
- E2W' = E2W @ T_local
- S2W' = S2W @ (S2E^{-1} @ T_local @ S2E)
Examples
--------
>>> T = np.eye(4)
>>> T[:3, 3] = [1, 0, 0] # Translation
>>> scene.ego.apply_T_sequence(T, referential="world")
"""
ego, cams, lids, rads = self._rig_lists()
n_total = len(ego.trajectory.timestamps)
i0, i1 = self._window(n_total, start_idx, end_idx)
if referential == "world":
ego2, sens2 = apply_world_to_rig(
ego_traj=ego.trajectory,
sensor_trajs=[*(s.trajectory for s in (cams + lids + rads))],
T_world=T,
start_idx=i0,
end_idx=i1,
coverage=coverage,
)
elif referential == "local":
ego2, sens2 = apply_local_to_rig(
ego_traj=ego.trajectory,
sensor_trajs=[*(s.trajectory for s in (cams + lids + rads))],
sensor_extrinsics=[s.extrinsics for s in (cams + lids + rads)],
T_local=T,
start_idx=i0,
end_idx=i1,
mode=mode,
)
else:
raise ValueError("referential must be 'world' or 'local'.")
cams_n = sens2[: len(cams)]
lids_n = sens2[len(cams) : len(cams) + len(lids)]
rads_n = sens2[len(cams) + len(lids) :]
self._assign(ego2, list(cams_n), list(lids_n), list(rads_n), recompute_vel=True)
return self
[docs]
@log_method
def apply_T(self, T: np.ndarray, **kwargs) -> "EgoEditor":
"""Apply a single 4x4 transformation matrix.
Alias for :meth:`apply_T_sequence` when passing a single transform.
Parameters
----------
T : np.ndarray
4x4 transformation matrix.
**kwargs
Additional arguments passed to :meth:`apply_T_sequence`.
Returns
-------
EgoEditor
Self for method chaining.
"""
return self.apply_T_sequence(T, **kwargs)
[docs]
@check_value_limits
@log_method
def shift(
self,
*,
forward: float = 0.0,
backward: float = 0.0,
left: float = 0.0,
right: float = 0.0,
up: float = 0.0,
down: float = 0.0,
start_idx: int | None = None,
end_idx: int | None = None,
referential: str = "local",
) -> "EgoEditor":
"""Translate the ego rig using semantic direction components.
Applies a translation to the ego vehicle and all attached sensors.
Parameters
----------
forward : float, optional
Distance forward (positive X) in meters. Default is 0.
backward : float, optional
Distance backward (negative X) in meters. Default is 0.
left : float, optional
Distance left (positive Y) in meters. Default is 0.
right : float, optional
Distance right (negative Y) in meters. Default is 0.
up : float, optional
Distance up (positive Z) in meters. Default is 0.
down : float, optional
Distance down (negative Z) in meters. Default is 0.
start_idx : int | None, optional
First frame to shift (inclusive). None means all frames.
end_idx : int | None, optional
Last frame to shift (exclusive). None means all frames.
referential : {"local", "world"}, optional
Coordinate frame:
- ``"local"``: Components in ego axes (default)
- ``"world"``: Components in world axes
Returns
-------
EgoEditor
Self for method chaining.
Examples
--------
>>> scene.ego.shift(forward=5) # Move forward 5m
>>> scene.ego.shift(left=2, up=0.5) # Move left and up
>>> scene.ego.shift(forward=3, start_idx=10) # From frame 10 onwards
See Also
--------
translate : Alias for shift.
transform : Combined translation and rotation.
"""
return self.transform(
forward=forward,
backward=backward,
left=left,
right=right,
up=up,
down=down,
start_idx=start_idx,
end_idx=end_idx,
referential=referential,
)
[docs]
@check_value_limits
@log_method
def translate(self, **kwargs) -> "EgoEditor":
"""Translate the ego rig.
Alias for :meth:`shift`.
Parameters
----------
**kwargs
Arguments passed to :meth:`shift`.
Returns
-------
EgoEditor
Self for method chaining.
"""
return self.shift(**kwargs)
[docs]
@check_value_limits
@log_method
def rotate(
self,
*,
roll: float = 0.0,
pitch: float = 0.0,
yaw: float = 0.0,
start_idx: int | None = None,
end_idx: int | None = None,
referential: str = "local",
) -> "EgoEditor":
"""Rotate the ego rig using Euler angles.
Applies a rotation to the ego vehicle and all attached sensors.
Parameters
----------
roll : float, optional
Rotation around X (forward) axis in degrees. Default is 0.
pitch : float, optional
Rotation around Y (left) axis in degrees. Default is 0.
yaw : float, optional
Rotation around Z (up) axis in degrees. Default is 0.
start_idx : int | None, optional
First frame to rotate (inclusive). None means all frames.
end_idx : int | None, optional
Last frame to rotate (exclusive). None means all frames.
referential : {"local", "world"}, optional
Coordinate frame. Default is ``"local"``.
Returns
-------
EgoEditor
Self for method chaining.
Examples
--------
>>> scene.ego.rotate(yaw=15) # Turn 15° left
>>> scene.ego.rotate(roll=5, pitch=-2) # Combined rotation
"""
return self.transform(
roll=roll,
pitch=pitch,
yaw=yaw,
start_idx=start_idx,
end_idx=end_idx,
referential=referential,
)
[docs]
def yaw_deg(self, deg: float, **kw) -> "EgoEditor":
"""Rotate the ego rig around the Z (up) axis.
Shortcut for ``rotate(yaw=deg)``.
Parameters
----------
deg : float
Yaw angle in degrees.
**kw
Additional arguments passed to :meth:`rotate`.
Returns
-------
EgoEditor
Self for method chaining.
"""
return self.rotate(yaw=deg, **kw)
[docs]
def pitch_deg(self, deg: float, **kw) -> "EgoEditor":
"""Rotate the ego rig around the Y (left) axis.
Shortcut for ``rotate(pitch=deg)``.
Parameters
----------
deg : float
Pitch angle in degrees.
**kw
Additional arguments passed to :meth:`rotate`.
Returns
-------
EgoEditor
Self for method chaining.
"""
return self.rotate(pitch=deg, **kw)
[docs]
def roll_deg(self, deg: float, **kw) -> "EgoEditor":
"""Rotate the ego rig around the X (forward) axis.
Shortcut for ``rotate(roll=deg)``.
Parameters
----------
deg : float
Roll angle in degrees.
**kw
Additional arguments passed to :meth:`rotate`.
Returns
-------
EgoEditor
Self for method chaining.
"""
return self.rotate(roll=deg, **kw)
[docs]
@check_value_limits
@log_method
def spin(
self,
*,
period: int = 10,
axis: str = "z",
start_idx: int | None = None,
end_idx: int | None = None,
) -> "EgoEditor":
"""Apply continuous rotation (spinning) to the ego rig.
Makes the ego vehicle rotate continuously around the specified
axis. Useful for testing or special effects.
Parameters
----------
period : int, optional
Frames per full revolution. Default is 10.
axis : {"x", "y", "z"}, optional
Rotation axis. Default is ``"z"`` (vertical).
start_idx : int | None, optional
First frame to apply spin (inclusive). None means 0.
end_idx : int | None, optional
Last frame to apply spin (exclusive). None means end.
Returns
-------
EgoEditor
Self for method chaining.
Raises
------
ValueError
If period is not positive or indices are invalid.
Examples
--------
>>> scene.ego.spin(period=20, axis="z") # One revolution per 20 frames
"""
ego, *_ = self._rig_lists()
n_total = len(ego.trajectory.timestamps)
idx_0 = 0 if start_idx is None else int(start_idx)
idx_1 = n_total if end_idx is None else int(end_idx)
if not (0 <= idx_0 < idx_1 <= n_total):
raise ValueError(
f"Invalid index window [{idx_0}, {idx_1}) for length {n_total}."
)
if period <= 0:
raise ValueError("period must be > 0")
segN = idx_1 - idx_0
ang_per_frame = 360.0 / float(period)
T_seq = np.zeros((segN, 4, 4), dtype=float)
for k in range(segN):
a = ang_per_frame * k
if axis == "x":
R = euler_xyz_deg(a, 0.0, 0.0)
elif axis == "y":
R = euler_xyz_deg(0.0, a, 0.0)
else: # default "z"
R = euler_xyz_deg(0.0, 0.0, a)
T_seq[k] = make_T(R, np.zeros(3))
return self.apply_T_sequence(
T_seq, start_idx=idx_0, end_idx=idx_1, referential="local"
)
# -------- maneuvers (rig-coupled) --------
[docs]
@check_value_limits
@log_method
def smooth_lane_change(
self,
*,
start_idx: int,
duration: int,
left: float = 0.0,
right: float = 0.0,
profile: str = "min_jerk",
kind: str = "smooth",
param: str = "index",
constant_speed: bool = True,
yaw_window: int | None = 9,
yaw_poly: int = 2,
) -> "EgoEditor":
"""Perform a smooth lane change maneuver.
Generates a smooth lateral displacement to simulate a lane
change, with proper heading adjustments.
Parameters
----------
start_idx : int
Frame index to start the maneuver.
duration : int
Number of frames for the maneuver.
left : float, optional
Lateral distance left in meters. Default is 0.
right : float, optional
Lateral distance right in meters. Default is 0.
profile : {"min_jerk", "bezier"}, optional
Curve profile:
- ``"min_jerk"``: Minimum-jerk trajectory (default)
- ``"bezier"``: Bezier curve
kind : {"smooth", "evasive"}, optional
Maneuver type:
- ``"smooth"``: Step to new lane position (default)
- ``"evasive"``: Return to original lane after swerve
param : {"index", "arc"}, optional
Parameterization method. Default is ``"index"``.
constant_speed : bool, optional
Maintain constant speed during maneuver. Default is True.
yaw_window : int | None, optional
Window size for yaw smoothing. Default is 9.
yaw_poly : int, optional
Polynomial order for yaw smoothing. Default is 2.
Returns
-------
EgoEditor
Self for method chaining.
Examples
--------
>>> # Change to left lane
>>> scene.ego.smooth_lane_change(start_idx=10, duration=30, left=3.5)
>>> # Temporary evasive swerve
>>> scene.ego.smooth_lane_change(
... start_idx=10, duration=30, left=2, kind="evasive"
... )
"""
distance = float(left or -right)
if abs(distance) < 1e-12:
return self
ego, *_ = self._rig_lists()
T_seq, tail_y = build_lane_change_T_seq(
ego_object2world=ego.trajectory.object2world,
start_idx=int(start_idx),
duration=int(duration),
distance_m=distance,
kind="evasive" if kind == "evasive" else "smooth",
profile="min_jerk" if profile == "min_jerk" else "bezier",
param="arc" if param == "arc" else "index",
constant_speed=bool(constant_speed),
yaw_savgol_window=yaw_window,
yaw_savgol_poly=yaw_poly,
)
self.apply_T_sequence(
T_seq,
start_idx=int(start_idx),
end_idx=int(start_idx) + int(duration),
referential="local",
)
# persist lateral tail only for 'smooth' step changes
if kind != "evasive" and abs(tail_y) > 1e-12:
i1 = int(start_idx) + int(duration)
if i1 < len(ego.trajectory.timestamps):
if tail_y >= 0:
self.transform(left=tail_y, start_idx=i1, end_idx=None)
else:
self.transform(right=-tail_y, start_idx=i1, end_idx=None)
return self
[docs]
@check_value_limits
@log_method
def evasive_maneuver(
self,
*,
start_idx: int,
duration: int,
left: float = 0.0,
right: float = 0.0,
) -> "EgoEditor":
"""Perform an evasive maneuver that returns to the original lane.
Generates a swerve maneuver where the ego moves laterally and
then returns to its original lane position.
Parameters
----------
start_idx : int
Frame index to start the maneuver.
duration : int
Number of frames for the complete maneuver.
left : float, optional
Maximum lateral distance left in meters. Default is 0.
right : float, optional
Maximum lateral distance right in meters. Default is 0.
Returns
-------
EgoEditor
Self for method chaining.
Examples
--------
>>> scene.ego.evasive_maneuver(start_idx=20, duration=40, left=2)
See Also
--------
smooth_lane_change : General lane change with options.
"""
distance = float(left or -right)
if abs(distance) < 1e-12:
return self
ego, *_ = self._rig_lists()
T_seq, _ = build_lane_change_T_seq(
ego_object2world=ego.trajectory.object2world,
start_idx=int(start_idx),
duration=int(duration),
distance_m=distance,
kind="evasive",
)
start_index = int(start_idx)
end_index = start_index + int(duration)
return self.apply_T_sequence(
T_seq, start_idx=start_index, end_idx=end_index, referential="local"
)
def _verify_orientation(self, scene, cam) -> str:
"""Determine camera coordinate convention by checking forward axis alignment.
Parameters
----------
scene : Scene
The scene containing the camera.
cam : Camera
The camera to check.
Returns
-------
str
``"vehicle"`` or ``"opencv"`` based on detected convention.
"""
E2W0 = np.asarray(scene.ego_vehicle.trajectory.object2world, float)[0]
S2W0 = np.asarray(cam.trajectory.object2world, float)[0]
fwd_ego = E2W0[:3, 0]
fwd_ego /= np.linalg.norm(fwd_ego) + 1e-12
fwd_if_vehicle = S2W0[:3, :3] @ np.array([1, 0, 0])
fwd_if_opencv = S2W0[:3, :3] @ np.array([0, 0, 1])
a = float(
np.dot(fwd_if_vehicle / (np.linalg.norm(fwd_if_vehicle) + 1e-12), fwd_ego)
)
b = float(
np.dot(fwd_if_opencv / (np.linalg.norm(fwd_if_opencv) + 1e-12), fwd_ego)
)
return "opencv" if b > a + 0.2 else "vehicle"
[docs]
@log_method
def normalize_camera_conventions(
self,
*,
to: str = "vehicle",
verify: bool = True,
) -> "EgoEditor":
"""Convert all camera extrinsics to a consistent convention.
Normalizes all cameras to use the same coordinate convention,
ensuring consistent behavior across the sensor rig.
Parameters
----------
to : {"vehicle", "opencv"}, optional
Target coordinate convention. Default is ``"vehicle"``.
verify : bool, optional
If True (default), verify each camera's current convention
before converting.
Returns
-------
EgoEditor
Self for method chaining.
Notes
-----
Coordinate conventions:
- **Vehicle**: X-forward, Y-left, Z-up
- **OpenCV**: X-right, Y-down, Z-forward
Examples
--------
>>> scene.ego.normalize_camera_conventions(to="vehicle")
"""
sensors = getattr(self.scene.ego_vehicle, "sensors", None)
cams = [] if sensors is None else (sensors.cameras or [])
if not cams:
return self
for cam in cams:
model = cam.camera_model.lower()
if model == to or model == "orthophoto":
continue
if verify:
probe = self._verify_orientation(self.scene, cam)
if model != probe:
model = probe
SensorEditor(scene=self.scene, sensor=cam).convert_convention(
from_convention=model, to_convention=to
).recompute_world_poses()
return self