from __future__ import annotations
|
|
import importlib.util
|
import sys
|
import unittest
|
from pathlib import Path
|
|
import numpy as np
|
|
|
SCRIPT = Path(__file__).parents[1] / "triangulate_with_rtk_lrf_poses.py"
|
sys.path.insert(0, str(SCRIPT.parent))
|
SPEC = importlib.util.spec_from_file_location("rtk_lrf_pose_triangulation", SCRIPT)
|
MODULE = importlib.util.module_from_spec(SPEC)
|
assert SPEC.loader is not None
|
SPEC.loader.exec_module(MODULE)
|
|
|
class RtkLrfPoseTriangulationTests(unittest.TestCase):
|
def test_upright_pose_looks_at_target(self) -> None:
|
centre = np.asarray([10.0, 20.0, 30.0])
|
target = np.asarray([30.0, 60.0, 10.0])
|
pose = MODULE.upright_cam_from_world(centre, target)
|
np.testing.assert_allclose(pose.inverse().translation, centre, atol=1e-10)
|
forward = pose.rotation.matrix().T @ np.asarray([0.0, 0.0, 1.0])
|
expected = (target - centre) / np.linalg.norm(target - centre)
|
np.testing.assert_allclose(forward, expected, atol=1e-10)
|
|
|
if __name__ == "__main__":
|
unittest.main()
|
