Spaces:
Running
Running
File size: 7,383 Bytes
499e141 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 |
import numpy as np
import pytest
from transforms3d.euler import euler2quat
from transforms3d.quaternions import axangle2quat, qmult, quat2mat, rotate_vector
from benchmark.metrics import Inputs, MetricManager
from benchmark.reprojection import project
from benchmark.utils import VARIANTS_ANGLE_COS, VARIANTS_ANGLE_SIN
def createInput(q_gt=None, t_gt=None, q_est=None, t_est=None, confidence=None, K=None, W=None, H=None):
q_gt = np.zeros(4) if q_gt is None else q_gt
t_gt = np.zeros(3) if t_gt is None else t_gt
q_est = np.zeros(4) if q_est is None else q_est
t_est = np.zeros(3) if t_est is None else t_est
confidence = 0. if confidence is None else confidence
K = np.eye(3) if K is None else K
H = 1 if H is None else H
W = 1 if W is None else W
return Inputs(q_gt=q_gt, t_gt=t_gt, q_est=q_est, t_est=t_est, confidence=confidence, K=K, W=W, H=H)
def randomQuat():
angles = np.random.uniform(0, 2*np.pi, 3)
q = euler2quat(*angles)
return q
class TestMetrics:
@pytest.mark.parametrize('run_number', range(50))
def test_t_err_tinvariance(self, run_number: int) -> None:
"""Computes the translation error given an initial translation and displacement of this
translation. The translation error must be equal to the norm of the displacement."""
mean, var = 5, 10
t0 = np.random.normal(mean, var, (3,))
displacement = np.random.normal(mean, var, (3,))
i = createInput(t_gt=t0, t_est=t0+displacement)
trans_err = MetricManager.trans_err(i)
assert np.isclose(trans_err, np.linalg.norm(displacement))
@pytest.mark.parametrize('run_number', range(50))
def test_trans_err_rinvariance(self, run_number: int) -> None:
"""Computes the translation error given estimated and gt vectors.
The translation error must be the same for a rotated version of those vectors
(same random rotation)"""
mean, var = 5, 10
t0 = np.random.normal(mean, var, (3,))
t1 = np.random.normal(mean, var, (3,))
q = randomQuat()
i = createInput(t_gt=t0, t_est=t1)
trans_err = MetricManager.trans_err(i)
ir = createInput(t_gt=rotate_vector(t0, q), t_est=rotate_vector(t1, q))
trans_err_r = MetricManager.trans_err(ir)
assert np.isclose(trans_err, trans_err_r)
@pytest.mark.parametrize('run_number', range(50))
@pytest.mark.parametrize('dtype', (np.float64, np.float32))
def test_rot_err_raxis(self, run_number: int, dtype: type) -> None:
"""Test rotation error for rotations around a random axis.
Note: We create GT as high precision, and only downcast when calling rot_err.
"""
q = randomQuat().astype(np.float64)
axis = np.random.uniform(low=-1, high=1, size=3).astype(np.float64)
angle = np.float64(np.random.uniform(low=-np.pi, high=np.pi))
qres = axangle2quat(vector=axis, theta=angle, is_normalized=False).astype(np.float64)
i = createInput(q_gt=q.astype(dtype), q_est=qmult(q, qres).astype(dtype))
rot_err = MetricManager.rot_err(i)
assert isinstance(rot_err, np.float64)
rot_err_expected = np.abs(np.degrees(angle))
# if we add up errors, we want them to be positive
assert 0. <= rot_err
rtol = 1.e-5 # numpy default
atol = 1.e-8 # numpy default
if isinstance(dtype, np.float32):
atol = 1.e-7 # 1/50 test might fail at 1.e-8
assert np.isclose(rot_err, rot_err_expected, rtol=rtol, atol=atol)
@pytest.mark.parametrize('run_number', range(50))
def test_r_err_mat(self, run_number: int) -> None:
q0 = randomQuat()
q1 = randomQuat()
i = createInput(q_gt=q0, q_est=q1)
rot_err = MetricManager.rot_err(i)
R0 = quat2mat(q0)
R1 = quat2mat(q1)
Rres = R1 @ R0.T
theta = (np.trace(Rres) - 1)/2
theta = np.clip(theta, -1, 1)
angle = np.degrees(np.arccos(theta))
assert np.isclose(angle, rot_err)
def test_reproj_error_identity(self):
"""Test that reprojection error is zero if poses match"""
q = randomQuat()
t = np.random.normal(0, 10, (3,))
i = createInput(q_gt=q, t_gt=t, q_est=q, t_est=t)
reproj_err = MetricManager.reproj_err(i)
assert np.isclose(reproj_err, 0)
@pytest.mark.parametrize('run_number', range(10))
@pytest.mark.parametrize('variant', (VARIANTS_ANGLE_SIN,))
@pytest.mark.parametrize('dtype', (np.float64,))
def test_r_err_small(self, run_number: int, variant: str, dtype: type) -> None:
"""Test rotation error for small angle differences.
Note: We create GT as high precision, and only downcast when calling rot_err.
"""
scales_failed = []
for scale in np.logspace(start=-1, stop=-9, num=9, base=10, dtype=dtype):
q = randomQuat().astype(np.float64)
angle = np.float64(np.random.uniform(low=-np.pi, high=np.pi)) * scale
assert isinstance(angle, np.float64)
axis = np.random.uniform(low=-1., high=1., size=3).astype(np.float64)
assert axis.dtype == np.float64
qres = axangle2quat(vector=axis, theta=angle, is_normalized=False).astype(np.float64)
assert qres.dtype == np.float64
i = createInput(q_gt=q.astype(dtype), q_est=qmult(q, qres).astype(dtype))
# We expect the error to always be np.float64 for highest acc.
rot_err = MetricManager.rot_err(i, variant=variant)
assert isinstance(rot_err, np.float64)
rot_err_expected = np.abs(np.degrees(angle))
assert isinstance(rot_err_expected, type(rot_err))
# if we add up errors, we want them to be positive
assert 0. <= rot_err
# check accuracy for one magnitude higher tolerance than the angle
tol = 0.1 * scale
# need to be more permissive for lower precision
if dtype == np.float32:
tol = 1.e3 * scale
# cast to dtype for checking
rot_err = rot_err.astype(dtype)
rot_err_expected = rot_err_expected.astype(dtype)
if variant == VARIANTS_ANGLE_SIN:
assert np.isclose(rot_err, rot_err_expected, rtol=tol, atol=tol)
elif variant == VARIANTS_ANGLE_COS:
if not np.isclose(rot_err, rot_err_expected, rtol=tol, atol=tol):
print(f"[variant '{variant}'] raises an error for\n"
f"\trot_err: {rot_err}"
f"\trot_err_expected: {rot_err_expected}"
f"\trtol: {tol}"
f"\tatol: {tol}")
scales_failed.append(scale)
if len(scales_failed):
pytest.fail(f"Variant {variant} failed at scales {scales_failed}")
def test_projection() -> None:
xyz = np.array(((10, 20, 30), (10, 30, 50), (-20, -15, 5),
(-20, -50, 10)), dtype=np.float32)
K = np.eye(3)
uv = np.array(((1/3, 2/3), (1/5, 3/5), (-4, -3),
(-2, -5)), dtype=np.float32)
assert np.allclose(uv, project(xyz, K))
uv = np.array(((1/3, 2/3), (1/5, 3/5), (0, 0), (0, 0)), dtype=np.float32)
assert np.allclose(uv, project(xyz, K, img_size=(5, 5)))
|