diff --git a/common/params_keys.h b/common/params_keys.h index b4f0eeadb0..b5479dd865 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -181,6 +181,7 @@ inline static std::unordered_map keys = { {"BorderWidth", {PERSISTENT, FLOAT, "100.0", "100.0", 2}}, {"CalibratedLateralAcceleration", {PERSISTENT, FLOAT, "2.0", "2.0", 2}}, {"CalibrationProgress", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, + {"CameraOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"CameraView", {PERSISTENT, INT, "3", "0", 2}}, {"CancelDownloadMaps", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, {"DisableWideRoad", {PERSISTENT, BOOL, "0", "0", 3}}, diff --git a/selfdrive/modeld/camera_offset.py b/selfdrive/modeld/camera_offset.py new file mode 100644 index 0000000000..ffdf3478ec --- /dev/null +++ b/selfdrive/modeld/camera_offset.py @@ -0,0 +1,51 @@ +from __future__ import annotations + +import numpy as np + + +DEFAULT_CAMERA_HEIGHT = 1.22 +CAMERA_OFFSET_SMOOTHING = 0.1 + + +def _device_cameras(): + from openpilot.common.transformations.camera import DEVICE_CAMERAS + + return DEVICE_CAMERAS + + +class CameraOffset: + def __init__(self) -> None: + self.target_offset = 0.0 + self.offset = 0.0 + + @staticmethod + def apply(model_transform: np.ndarray, intrinsics: np.ndarray, camera_height: float, offset: float) -> np.ndarray: + height = camera_height if camera_height > 0.0 else DEFAULT_CAMERA_HEIGHT + cy = intrinsics[1, 2] + shear = np.eye(3, dtype=np.float32) + shear[0, 1] = offset / height + shear[0, 2] = -offset / height * cy + return (shear @ model_transform).astype(np.float32) + + def set_target(self, offset: float) -> None: + self.target_offset = float(offset) + + def update( + self, + model_transform_main: np.ndarray, + model_transform_extra: np.ndarray, + device_type: str, + camera_sensor: str, + camera_height: float, + main_wide_camera: bool, + device_cameras: dict | None = None, + ) -> tuple[np.ndarray, np.ndarray]: + self.offset += (self.target_offset - self.offset) * CAMERA_OFFSET_SMOOTHING + + dc = (device_cameras or _device_cameras())[(device_type, camera_sensor)] + main_intrinsics = dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics + + return ( + self.apply(model_transform_main, main_intrinsics, camera_height, self.offset), + self.apply(model_transform_extra, dc.ecam.intrinsics, camera_height, self.offset), + ) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index ed3b101ff9..46592b3224 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -23,6 +23,7 @@ from openpilot.system import sentry from opendbc.car.car_helpers import get_demo_car_params from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan_tomb_raider, smooth_value +from openpilot.selfdrive.modeld.camera_offset import CameraOffset, DEFAULT_CAMERA_HEIGHT from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output from openpilot.selfdrive.modeld.constants import ModelConstants, Plan @@ -551,6 +552,8 @@ def main(demo=False): buf_main, buf_extra = None, None meta_main = FrameMeta() meta_extra = FrameMeta() + camera_offset = CameraOffset() + camera_offset.set_target(params.get_float("CameraOffset", return_default=True)) if demo: @@ -608,11 +611,23 @@ def main(demo=False): v_ego = max(sm["carState"].vEgo, 0.) lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32) + if sm.frame % 60 == 0: + camera_offset.set_target(params.get_float("CameraOffset", return_default=True)) + if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32) model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32) + camera_height = sm["liveCalibration"].height[0] if sm["liveCalibration"].height else DEFAULT_CAMERA_HEIGHT + model_transform_main, model_transform_extra = camera_offset.update( + model_transform_main, + model_transform_extra, + str(sm["deviceState"].deviceType), + str(sm["roadCameraState"].sensor), + camera_height, + main_wide_camera, + ) live_calib_seen = True traffic_convention = np.zeros(2) diff --git a/selfdrive/modeld/modeld_v16.py b/selfdrive/modeld/modeld_v16.py index bfd31764b2..b2a50a90ee 100644 --- a/selfdrive/modeld/modeld_v16.py +++ b/selfdrive/modeld/modeld_v16.py @@ -30,6 +30,7 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan, smooth_value +from openpilot.selfdrive.modeld.camera_offset import CameraOffset, DEFAULT_CAMERA_HEIGHT from openpilot.selfdrive.modeld.compile_modeld import POLICY_INPUTS, make_input_queues from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.fill_model_msg import PublishState, fill_model_msg, fill_pose_msg @@ -338,6 +339,8 @@ def main(demo=False): buf_main, buf_extra = None, None meta_main = FrameMeta() meta_extra = FrameMeta() + camera_offset = CameraOffset() + camera_offset.set_target(params.get_float("CameraOffset", return_default=True)) if demo: CP = get_demo_car_params() @@ -387,6 +390,8 @@ def main(demo=False): frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.0) lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS + if sm.frame % 60 == 0: + camera_offset.set_target(params.get_float("CameraOffset", return_default=True)) if sm.updated["liveCalibration"] and sm.seen["roadCameraState"] and sm.seen["deviceState"]: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) @@ -397,6 +402,15 @@ def main(demo=False): False, ).astype(np.float32) model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32) + camera_height = sm["liveCalibration"].height[0] if sm["liveCalibration"].height else DEFAULT_CAMERA_HEIGHT + model_transform_main, model_transform_extra = camera_offset.update( + model_transform_main, + model_transform_extra, + str(sm["deviceState"].deviceType), + str(sm["roadCameraState"].sensor), + camera_height, + main_wide_camera, + ) live_calib_seen = True traffic_convention = np.zeros(2, dtype=np.float32) diff --git a/selfdrive/modeld/tests/test_camera_offset.py b/selfdrive/modeld/tests/test_camera_offset.py new file mode 100644 index 0000000000..dca6f198c8 --- /dev/null +++ b/selfdrive/modeld/tests/test_camera_offset.py @@ -0,0 +1,76 @@ +from types import SimpleNamespace + +import numpy as np + +from openpilot.selfdrive.modeld.camera_offset import CameraOffset + + +def _camera_meta(): + fcam_intrinsics = np.array([ + [910.0, 0.0, 582.0], + [0.0, 910.0, 437.0], + [0.0, 0.0, 1.0], + ], dtype=np.float32) + ecam_intrinsics = np.array([ + [560.0, 0.0, 582.0], + [0.0, 560.0, 437.0], + [0.0, 0.0, 1.0], + ], dtype=np.float32) + return { + ("mici", "os04c10"): SimpleNamespace( + fcam=SimpleNamespace(intrinsics=fcam_intrinsics), + ecam=SimpleNamespace(intrinsics=ecam_intrinsics), + ) + } + + +def test_camera_offset_smoothing(): + camera_offset = CameraOffset() + camera_offset.set_target(0.2) + + transform = np.eye(3, dtype=np.float32) + camera_offset.update(transform, transform, "mici", "os04c10", 1.22, False, _camera_meta()) + np.testing.assert_almost_equal(camera_offset.offset, 0.02) + + camera_offset.update(transform, transform, "mici", "os04c10", 1.22, False, _camera_meta()) + np.testing.assert_almost_equal(camera_offset.offset, 0.038) + + +def test_camera_offset_apply(): + intrinsics = _camera_meta()[("mici", "os04c10")].fcam.intrinsics + transform = np.eye(3, dtype=np.float32) + height = 1.22 + offset = 0.1 + + cy = intrinsics[1, 2] + expected = np.eye(3, dtype=np.float32) + expected[0, 1] = offset / height + expected[0, 2] = -offset / height * cy + + result = CameraOffset.apply(transform, intrinsics, height, offset) + np.testing.assert_array_almost_equal(result, expected) + + +def test_camera_offset_update_default_noop(): + camera_offset = CameraOffset() + main_transform = np.eye(3, dtype=np.float32) + extra_transform = np.eye(3, dtype=np.float32) + + main_out, extra_out = camera_offset.update(main_transform, extra_transform, "mici", "os04c10", 1.22, False, _camera_meta()) + + np.testing.assert_array_equal(main_out, main_transform) + np.testing.assert_array_equal(extra_out, extra_transform) + + +def test_camera_offset_update_changes_both_transforms(): + camera_offset = CameraOffset() + camera_offset.set_target(0.2) + main_transform = np.eye(3, dtype=np.float32) + extra_transform = np.eye(3, dtype=np.float32) + + main_out, extra_out = camera_offset.update(main_transform, extra_transform, "mici", "os04c10", 1.22, False, _camera_meta()) + + assert not np.array_equal(main_out, main_transform) + assert not np.array_equal(extra_out, extra_transform) + assert main_out[0, 1] != 0.0 + assert main_out[0, 2] != 0.0 diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index 30f8b130db..36cdc29dc2 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -3394,6 +3394,17 @@ "data_type": "bool", "ui_type": "toggle" }, + { + "key": "CameraOffset", + "label": "Camera Offset", + "description": "Virtually shift the camera perspective used by the driving model. Positive values bias the model center left; negative values bias it right. Use only for development.", + "data_type": "float", + "ui_type": "numeric", + "min": -0.35, + "max": 0.35, + "step": 0.01, + "precision": 2 + }, { "key": "AllowImpossibleAcceleration", "label": "Allow Impossible Acceleration",