mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-15 00:03:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

camera offset: use real horizon for the shear center (#2016)

Browse Source
This commit is contained in:
dzid26
2026-09-13 20:38:20 +01:00
committed by GitHub
parent c57f9a7f4e
commit f96c40cc58
2 changed files with 54 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/sunnypilot/modeld_v2/camera_offset_helper.py
+15 -4
View File
@@ -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
openpilot/sunnypilot/modeld_v2/tests/test_camera_offset_helper.py
+39 -9
View File
@@ -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)