from __future__ import annotations
|
|
import importlib.util
|
import unittest
|
from pathlib import Path
|
|
import numpy as np
|
|
|
SCRIPT = Path(__file__).parents[1] / "diagnose_photo_pose_alignment.py"
|
SPEC = importlib.util.spec_from_file_location("photo_pose_alignment", SCRIPT)
|
MODULE = importlib.util.module_from_spec(SPEC)
|
assert SPEC.loader is not None
|
SPEC.loader.exec_module(MODULE)
|
|
|
class PhotoPoseAlignmentTests(unittest.TestCase):
|
def test_similarity_alignment_recovers_metric_transform(self) -> None:
|
source = np.asarray([[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [0.0, 3.0, 1.0], [1.0, 2.0, 4.0]])
|
expected_rotation = np.asarray([[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]])
|
target = (2.5 * (expected_rotation @ source.T)).T + np.asarray([10.0, -4.0, 7.0])
|
scale, rotation, translation = MODULE.similarity_alignment(source, target)
|
actual = (scale * (rotation @ source.T)).T + translation
|
self.assertAlmostEqual(scale, 2.5, places=8)
|
np.testing.assert_allclose(actual, target, atol=1e-8)
|
|
def test_wgs84_to_enu_keeps_origin_at_zero(self) -> None:
|
origin = np.asarray([25.0, 113.0, 100.0])
|
enu = MODULE.wgs84_to_enu(np.asarray([origin, [25.0001, 113.0001, 105.0]]), origin)
|
np.testing.assert_allclose(enu[0], np.zeros(3), atol=1e-6)
|
self.assertGreater(enu[1, 0], 0.0)
|
self.assertGreater(enu[1, 1], 0.0)
|
self.assertGreater(enu[1, 2], 4.9)
|
|
|
if __name__ == "__main__":
|
unittest.main()
|
