Mom's Spaghetti

common/params_keys.h

@@ -181,6 +181,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},

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),
+    )

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)

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)

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

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",