crosstyan
d86e9f11d7
Switch the triangulation adapter from ad hoc Python dictionaries to rpt.make_camera so the tracker passes the exact typed camera objects expected by the current RPT bindings. The camera convention test now asserts against the bound camera object's numeric fields instead of raw dict payloads, which keeps the test aligned with the compiled interface.
150 lines
4.8 KiB
Python
150 lines
4.8 KiB
Python
import json
|
|
from pathlib import Path
|
|
from typing import NamedTuple, cast
|
|
|
|
import cv2
|
|
import numpy as np
|
|
import pytest
|
|
|
|
pytest.importorskip("rpt")
|
|
import rpt
|
|
|
|
from pose_tracking_exp.schema import CameraCalibration, CameraModel, SceneConfig, parse_camera_model
|
|
from pose_tracking_exp.tracking.replay_io import load_scene_file
|
|
from pose_tracking_exp.tracking.rpt_adapter import build_rpt_config
|
|
|
|
|
|
class _CameraArgs(NamedTuple):
|
|
name: str
|
|
width: int
|
|
height: int
|
|
K: np.ndarray
|
|
DC: np.ndarray
|
|
model: CameraModel
|
|
|
|
|
|
def _camera_args() -> _CameraArgs:
|
|
return _CameraArgs(
|
|
name="cam0",
|
|
width=640,
|
|
height=480,
|
|
K=np.asarray([[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]], dtype=np.float64),
|
|
DC=np.zeros(5, dtype=np.float64),
|
|
model=parse_camera_model("pinhole"),
|
|
)
|
|
|
|
|
|
def test_from_opencv_extrinsics_derives_rpt_pose() -> None:
|
|
args = _camera_args()
|
|
rotation_vec = np.asarray([0.0, 0.2, 0.0], dtype=np.float64).reshape(3, 1)
|
|
rotation, _ = cv2.Rodrigues(rotation_vec)
|
|
translation = np.asarray([0.5, -0.1, 2.0], dtype=np.float64)
|
|
|
|
camera = CameraCalibration.from_opencv_extrinsics(
|
|
name=args.name,
|
|
width=args.width,
|
|
height=args.height,
|
|
K=args.K,
|
|
DC=args.DC,
|
|
model=args.model,
|
|
R=rotation,
|
|
T=translation,
|
|
rvec=rotation_vec.reshape(3),
|
|
)
|
|
|
|
np.testing.assert_allclose(camera.pose_R, rotation.T)
|
|
np.testing.assert_allclose(camera.pose_T, -(rotation.T @ translation))
|
|
|
|
|
|
def test_from_rpt_pose_derives_opencv_extrinsics() -> None:
|
|
args = _camera_args()
|
|
pose_rotation_vec = np.asarray([0.0, -0.3, 0.0], dtype=np.float64).reshape(3, 1)
|
|
pose_rotation, _ = cv2.Rodrigues(pose_rotation_vec)
|
|
pose_translation = np.asarray([1.5, 0.2, -0.4], dtype=np.float64)
|
|
|
|
camera = CameraCalibration.from_rpt_pose(
|
|
name=args.name,
|
|
width=args.width,
|
|
height=args.height,
|
|
K=args.K,
|
|
DC=args.DC,
|
|
model=args.model,
|
|
R=pose_rotation,
|
|
T=pose_translation,
|
|
)
|
|
|
|
np.testing.assert_allclose(camera.pose_R, pose_rotation)
|
|
np.testing.assert_allclose(camera.pose_T, pose_translation)
|
|
np.testing.assert_allclose(camera.R, pose_rotation.T)
|
|
np.testing.assert_allclose(camera.T, -(pose_rotation.T @ pose_translation))
|
|
|
|
|
|
def test_load_scene_file_supports_explicit_rpt_pose(tmp_path: Path) -> None:
|
|
scene_path = tmp_path / "scene.json"
|
|
payload = {
|
|
"extrinsic_format": "rpt_camera_pose",
|
|
"room_size": [6.0, 4.0, 3.0],
|
|
"room_center": [0.0, 0.0, 1.0],
|
|
"cameras": [
|
|
{
|
|
"name": "cam0",
|
|
"width": 640,
|
|
"height": 480,
|
|
"K": [[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]],
|
|
"DC": [0.0, 0.0, 0.0, 0.0, 0.0],
|
|
"R": [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
|
|
"T": [[1.0], [2.0], [3.0]],
|
|
}
|
|
],
|
|
}
|
|
scene_path.write_text(json.dumps(payload), encoding="utf-8")
|
|
|
|
scene = load_scene_file(scene_path)
|
|
|
|
np.testing.assert_allclose(scene.cameras[0].pose_T, [1.0, 2.0, 3.0])
|
|
np.testing.assert_allclose(scene.cameras[0].T, [-1.0, -2.0, -3.0])
|
|
|
|
|
|
def test_build_rpt_config_uses_pose_convention(monkeypatch: pytest.MonkeyPatch) -> None:
|
|
args = _camera_args()
|
|
camera = CameraCalibration.from_opencv_extrinsics(
|
|
name=args.name,
|
|
width=args.width,
|
|
height=args.height,
|
|
K=args.K,
|
|
DC=args.DC,
|
|
model=args.model,
|
|
R=np.eye(3, dtype=np.float64),
|
|
T=np.asarray([1.0, 2.0, 3.0], dtype=np.float64),
|
|
rvec=np.zeros(3, dtype=np.float64),
|
|
)
|
|
scene = SceneConfig(
|
|
room_size=np.asarray([6.0, 4.0, 3.0], dtype=np.float64),
|
|
room_center=np.asarray([0.0, 0.0, 1.0], dtype=np.float64),
|
|
cameras=(camera,),
|
|
)
|
|
captured: dict[str, object] = {}
|
|
|
|
def fake_make_triangulation_config(
|
|
cameras: list[rpt.Camera],
|
|
roomparams: np.ndarray,
|
|
joint_names: list[str],
|
|
*,
|
|
min_match_score: float,
|
|
min_group_size: int,
|
|
) -> dict[str, object]:
|
|
captured["cameras"] = cameras
|
|
captured["roomparams"] = roomparams
|
|
captured["joint_names"] = joint_names
|
|
captured["min_match_score"] = min_match_score
|
|
captured["min_group_size"] = min_group_size
|
|
return captured
|
|
|
|
monkeypatch.setattr("pose_tracking_exp.tracking.rpt_adapter.rpt.make_triangulation_config", fake_make_triangulation_config)
|
|
|
|
build_rpt_config(scene, min_match_score=0.5, min_group_size=2)
|
|
|
|
camera_payload = cast(list[rpt.Camera], captured["cameras"])[0]
|
|
np.testing.assert_allclose(camera_payload.R, camera.pose_R.tolist())
|
|
np.testing.assert_allclose(camera_payload.T, camera.pose_T.reshape(3, 1).tolist())
|