diff --git a/openpilot/sunnypilot/modeld_v2/camera_offset_helper.py b/openpilot/sunnypilot/modeld_v2/camera_offset_helper.py index 648ba01086..bcf3a20c04 100644 --- a/openpilot/sunnypilot/modeld_v2/camera_offset_helper.py +++ b/openpilot/sunnypilot/modeld_v2/camera_offset_helper.py @@ -15,11 +15,19 @@ class CameraOffsetHelper: self.actual_camera_offset = 0.0 @staticmethod - def apply_camera_offset(model_transform, intrinsics, height, offset_param): + def get_v_horizon(intrinsics, rpy_calib): cy = intrinsics[1, 2] + if len(rpy_calib) == 3 and np.isfinite(rpy_calib).all(): + fy = intrinsics[1, 1] + pitch = rpy_calib[1] + return float(cy - fy * np.tan(pitch)) + return float(cy) + + @staticmethod + def apply_camera_offset(model_transform, height, offset_param, v_horizon): shear = np.eye(3, dtype=np.float32) shear[0, 1] = offset_param / height - shear[0, 2] = -offset_param / height * cy + shear[0, 2] = -offset_param / height * v_horizon model_transform = (shear @ model_transform).astype(np.float32) return model_transform @@ -30,10 +38,13 @@ class CameraOffsetHelper: self.actual_camera_offset = (0.9 * self.actual_camera_offset) + (0.1 * self.camera_offset) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))] height = sm["extrinsicsCalibration"].height[0] if sm['extrinsicsCalibration'].height else 1.22 + rpy_calib = sm['extrinsicsCalibration'].rpyCalib intrinsics_main = dc.wide_road.intrinsics if main_wide_camera else dc.narrow_road.intrinsics - model_transform_main = self.apply_camera_offset(model_transform_main, intrinsics_main, height, self.actual_camera_offset) + v_horizon_main = self.get_v_horizon(intrinsics_main, rpy_calib) + model_transform_main = self.apply_camera_offset(model_transform_main, height, self.actual_camera_offset, v_horizon_main) intrinsics_extra = dc.wide_road.intrinsics - model_transform_extra = self.apply_camera_offset(model_transform_extra, intrinsics_extra, height, self.actual_camera_offset) + v_horizon_extra = self.get_v_horizon(intrinsics_extra, rpy_calib) + model_transform_extra = self.apply_camera_offset(model_transform_extra, height, self.actual_camera_offset, v_horizon_extra) return model_transform_main, model_transform_extra diff --git a/openpilot/sunnypilot/modeld_v2/tests/test_camera_offset_helper.py b/openpilot/sunnypilot/modeld_v2/tests/test_camera_offset_helper.py index 5398ac0ff1..4cd3634c13 100644 --- a/openpilot/sunnypilot/modeld_v2/tests/test_camera_offset_helper.py +++ b/openpilot/sunnypilot/modeld_v2/tests/test_camera_offset_helper.py @@ -6,8 +6,9 @@ See the LICENSE.md file in the root directory for more details. """ import numpy as np -from openpilot.common.transformations.camera import DEVICE_CAMERAS +from openpilot.common.transformations.camera import DEVICE_CAMERAS, view_frame_from_device_frame from openpilot.common.transformations.model import get_warp_matrix +from openpilot.common.transformations.orientation import rot_from_euler from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper from openpilot.common.test import OpenpilotTestCase @@ -46,29 +47,50 @@ class TestCameraOffset(OpenpilotTestCase): self.camera_offset.update(main_transform, extra_transform, sm, False) np.testing.assert_almost_equal(self.camera_offset.actual_camera_offset, 0.038) - def test_camera_offset_(self): + def test_apply_camera_offset(self): intrinsics = self.dc.narrow_road.intrinsics + v_horizon = CameraOffsetHelper.get_v_horizon(intrinsics, []) # pitch = 0 fallback: v_horizon == cy transform = np.eye(3, dtype=np.float32) height = 1.22 offset = 0.1 - cy = intrinsics[1, 2] expected_shear = np.eye(3, dtype=np.float32) expected_shear[0, 1] = offset / height - expected_shear[0, 2] = -offset / height * cy + expected_shear[0, 2] = -offset / height * v_horizon - result = CameraOffsetHelper.apply_camera_offset(transform, intrinsics, height, offset) + result = CameraOffsetHelper.apply_camera_offset(transform, height, offset, v_horizon) np.testing.assert_array_almost_equal(result, expected_shear) + def test_v_horizon_empty_rpy(self): + intrinsics = self.dc.narrow_road.intrinsics + v_horizon = CameraOffsetHelper.get_v_horizon(intrinsics, []) + np.testing.assert_almost_equal(v_horizon, intrinsics[1, 2]) + + def test_v_horizon_projection(self): + intrinsics = self.dc.narrow_road.intrinsics + f, cy = intrinsics[1, 1], intrinsics[1, 2] + + for pitch_deg in [6.0, -6.0, 0.0]: + rpy = [0.0, np.radians(pitch_deg), 0.0] + d_dev = rot_from_euler(rpy) @ np.array([1.0, 0.0, 0.0]) + view = view_frame_from_device_frame @ d_dev + expected = cy + f * view[1] / view[2] + + v_horizon = CameraOffsetHelper.get_v_horizon(intrinsics, rpy) + np.testing.assert_almost_equal(v_horizon, expected, decimal=4) + def test_update(self): + height = 1.2 + pitch = np.radians(-8.0) + sm = MockStruct( deviceState=MockStruct(deviceType='mici'), narrowRoadCameraState=MockStruct(sensor='os04c10'), - extrinsicsCalibration=MockStruct(rpyCalib=[0.0, 0.0, 0.0], height=[1.22]) + extrinsicsCalibration=MockStruct(rpyCalib=[0.0, pitch, 0.0], height=[height]) ) intrinsics_main = self.dc.narrow_road.intrinsics intrinsics_extra = self.dc.wide_road.intrinsics - device_from_calib_euler = np.array([0.0, 0.0, 0.0], dtype=np.float32) + device_from_calib_euler = np.array(sm['extrinsicsCalibration'].rpyCalib, dtype=np.float32) main_transform = get_warp_matrix(device_from_calib_euler, intrinsics_main, False).astype(np.float32) extra_transform = get_warp_matrix(device_from_calib_euler, intrinsics_extra, True).astype(np.float32) @@ -81,5 +103,13 @@ class TestCameraOffset(OpenpilotTestCase): main_out, extra_out = self.camera_offset.update(main_transform, extra_transform, sm, False) 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 + + # settle the low-pass filter + for _ in range(100): + main_out, extra_out = self.camera_offset.update(main_transform, extra_transform, sm, False) + + # undo main_transform dot product to get shear matrix + shear = main_out @ np.linalg.inv(main_transform) + expected_v_horizon = intrinsics_main[1, 2] - intrinsics_main[1, 1] * np.tan(pitch) + np.testing.assert_almost_equal(shear[0, 1], self.camera_offset.actual_camera_offset / height, decimal=4) + np.testing.assert_almost_equal(shear[0, 2], -self.camera_offset.actual_camera_offset / height * expected_v_horizon, decimal=4)