50 lines
1.3 KiB
Python
50 lines
1.3 KiB
Python
"""Unit test per umeyama_similarity (absolute orientation con scala)."""
|
|
from __future__ import annotations
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from presa_mano.geometry import umeyama_similarity, rpy_deg_to_quaternion_xyzw, quat_xyzw_to_R
|
|
|
|
|
|
def _known_transform():
|
|
# scala 1.5, rotazione nota, traslazione nota
|
|
R = quat_xyzw_to_R(rpy_deg_to_quaternion_xyzw(10.0, -20.0, 35.0))
|
|
s = 1.5
|
|
t = np.array([0.3, -0.1, 0.2])
|
|
return s, R, t
|
|
|
|
|
|
def test_recovers_known_similarity():
|
|
s, R, t = _known_transform()
|
|
rng = np.random.default_rng(0)
|
|
src = rng.normal(size=(8, 3))
|
|
dst = (s * (R @ src.T)).T + t
|
|
|
|
s_hat, R_hat, t_hat, rmse = umeyama_similarity(src, dst)
|
|
|
|
assert abs(s_hat - s) < 1e-6
|
|
assert np.allclose(R_hat, R, atol=1e-6)
|
|
assert np.allclose(t_hat, t, atol=1e-6)
|
|
assert rmse < 1e-6
|
|
|
|
|
|
def test_forbids_reflection():
|
|
rng = np.random.default_rng(1)
|
|
src = rng.normal(size=(5, 3))
|
|
dst = rng.normal(size=(5, 3))
|
|
_, R_hat, _, _ = umeyama_similarity(src, dst)
|
|
assert np.linalg.det(R_hat) > 0
|
|
|
|
|
|
def test_raises_on_too_few_points():
|
|
with pytest.raises(ValueError):
|
|
umeyama_similarity(np.zeros((2, 3)), np.zeros((2, 3)))
|
|
|
|
|
|
def test_raises_on_degenerate_points():
|
|
src = np.ones((4, 3))
|
|
dst = np.ones((4, 3)) * 2.0
|
|
with pytest.raises(ValueError):
|
|
umeyama_similarity(src, dst)
|