73 lines
2.0 KiB
Python
73 lines
2.0 KiB
Python
import numpy as np
|
|
import pyzed.sl as sl
|
|
from typing import Any, cast, TYPE_CHECKING
|
|
from aruco.svo_sync import SVOReader
|
|
|
|
if TYPE_CHECKING:
|
|
from _pytest.monkeypatch import MonkeyPatch
|
|
|
|
|
|
class FakeMat:
|
|
def __init__(self) -> None:
|
|
self.data: np.ndarray = np.array([])
|
|
|
|
def get_data(self) -> np.ndarray:
|
|
return self.data
|
|
|
|
|
|
class FakeInitParameters:
|
|
coordinate_units: sl.UNIT
|
|
|
|
def __init__(self, units: sl.UNIT) -> None:
|
|
self.coordinate_units = units
|
|
|
|
|
|
class FakeCamera:
|
|
init_params: FakeInitParameters
|
|
|
|
def __init__(self, units: sl.UNIT) -> None:
|
|
self.init_params = FakeInitParameters(units)
|
|
|
|
def get_init_parameters(self) -> FakeInitParameters:
|
|
return self.init_params
|
|
|
|
def retrieve_measure(self, _mat: Any, _measure: sl.MEASURE) -> sl.ERROR_CODE:
|
|
return sl.ERROR_CODE.SUCCESS
|
|
|
|
|
|
def test_retrieve_depth_unit_guard(monkeypatch: "MonkeyPatch") -> None:
|
|
# Setup SVOReader with depth enabled
|
|
reader = SVOReader([], depth_mode=sl.DEPTH_MODE.ULTRA)
|
|
|
|
# Mock depth data (e.g., 2.0 meters)
|
|
depth_data = np.full((100, 100), 2.0, dtype=np.float32)
|
|
|
|
def fake_mat_factory() -> FakeMat:
|
|
m = FakeMat()
|
|
m.data = depth_data
|
|
return m
|
|
|
|
monkeypatch.setattr("aruco.svo_sync.sl.Mat", fake_mat_factory)
|
|
|
|
# Case 1: Units are METER -> Should NOT divide by 1000
|
|
fake_cam_meter = FakeCamera(sl.UNIT.METER)
|
|
cam_meter = cast(sl.Camera, cast(object, fake_cam_meter))
|
|
|
|
depth_meter = reader._retrieve_depth(cam_meter) # pyright: ignore [reportPrivateUsage]
|
|
assert depth_meter is not None
|
|
assert np.allclose(depth_meter, 2.0)
|
|
|
|
# Case 2: Units are MILLIMETER -> Should divide by 1000
|
|
fake_cam_mm = FakeCamera(sl.UNIT.MILLIMETER)
|
|
cam_mm = cast(sl.Camera, cast(object, fake_cam_mm))
|
|
|
|
depth_mm = reader._retrieve_depth(cam_mm) # pyright: ignore [reportPrivateUsage]
|
|
assert depth_mm is not None
|
|
assert np.allclose(depth_mm, 0.002)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
import pytest
|
|
|
|
_ = pytest.main([__file__])
|