IQ.Pilot Prebuilt Release @ 27f668a

iqpilot/selfdrive/iqmodeld/camera.py

@@ -0,0 +1,70 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+import numpy as np
+
+from iqpilot.common.transformations.camera import DEVICE_CAMERAS
+
+MAX_CAMERA_OFFSET_METERS = 0.35
+
+
+class _OffsetSmoother:
+  def __init__(self, blend: float = 0.1):
+    self._blend = blend
+    self._value = 0.0
+
+  def step(self, target: float) -> float:
+    self._value = ((1.0 - self._blend) * self._value) + (self._blend * float(target))
+    return self._value
+
+
+def _clamped_offset(raw_offset) -> float:
+  try:
+    parsed = float(raw_offset)
+  except (TypeError, ValueError):
+    parsed = 0.0
+  return float(np.clip(parsed, -MAX_CAMERA_OFFSET_METERS, MAX_CAMERA_OFFSET_METERS))
+
+
+def _camera_profile(sm):
+  return DEVICE_CAMERAS[(str(sm["deviceState"].deviceType), str(sm["roadCameraState"].sensor))]
+
+
+def _calibration_height(sm) -> float:
+  from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_SANE_MIN, HEIGHT_SANE_MAX
+  h = sm["extrinsicsCalibration"].height[0] if sm["extrinsicsCalibration"].height else 1.22
+  return h if HEIGHT_SANE_MIN <= h <= HEIGHT_SANE_MAX else 1.22
+
+
+def _sheared_transform(model_transform, intrinsics, height: float, lateral_offset: float):
+  optical_center_y = intrinsics[1, 2]
+  projection_bias = np.eye(3, dtype=np.float32)
+  projection_bias[0, 1] = lateral_offset / height
+  projection_bias[0, 2] = -(lateral_offset / height) * optical_center_y
+  return (projection_bias @ model_transform).astype(np.float32)
+
+
+class CameraOffsetHelper:
+  def __init__(self):
+    self.camera_offset = 0.0
+    self.actual_camera_offset = 0.0
+    self._smoother = _OffsetSmoother()
+
+  @staticmethod
+  def apply_camera_offset(model_transform, intrinsics, height, offset_param):
+    return _sheared_transform(model_transform, intrinsics, height, offset_param)
+
+  def set_offset(self, offset):
+    self.camera_offset = _clamped_offset(offset)
+
+  def update(self, model_transform_main, model_transform_extra, sm, main_wide_camera, extra_uses_wide_camera=True):
+    self.actual_camera_offset = self._smoother.step(self.camera_offset)
+    camera_bundle = _camera_profile(sm)
+    camera_height = _calibration_height(sm)
+    main_intrinsics = camera_bundle.ecam.intrinsics if main_wide_camera else camera_bundle.fcam.intrinsics
+    extra_intrinsics = camera_bundle.ecam.intrinsics if extra_uses_wide_camera else camera_bundle.fcam.intrinsics
+
+    return (
+      self.apply_camera_offset(model_transform_main, main_intrinsics, camera_height, self.actual_camera_offset),
+      self.apply_camera_offset(model_transform_extra, extra_intrinsics, camera_height, self.actual_camera_offset),
+    )