100 lines
5.4 KiB
Python
100 lines
5.4 KiB
Python
"""Independent observer images may constrain a family, never waive ambiguity."""
|
|
|
|
from dataclasses import replace
|
|
import math
|
|
|
|
import numpy as np
|
|
import pytest
|
|
from scipy.spatial.transform import Rotation
|
|
|
|
from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points, solve_square_tag_ippe
|
|
from linkerhand_calibration.core.geometry.tag_pose.observer_pose import fit_observer_pose
|
|
from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose
|
|
|
|
|
|
@pytest.fixture(scope='module')
|
|
def observer_images():
|
|
matrix = np.array([[1600., 0, 640.], [0, 1600., 480.], [0, 0, 1.]])
|
|
parents, candidates, corners = [], [], []
|
|
relative = Rotation.identity()
|
|
translation = np.array([.045, .02, 0.])
|
|
for theta in np.linspace(-35, 35, 48):
|
|
rotation = Rotation.from_euler('xy', [theta, theta*.4], degrees=True)
|
|
origin = np.array([.015, -.02, .5])
|
|
parents.append(SquareTagPose(tuple(rotation.as_quat()), tuple(origin), 0.))
|
|
points = rotation.apply(relative.apply(square_object_points(.016))+translation)+origin
|
|
projected = points@matrix.T
|
|
pixels = projected[:, :2]/projected[:, 2:]
|
|
corners.append(pixels)
|
|
candidates.append(tuple(solve_square_tag_ippe(pixels, tag_size_m=.016, camera_matrix=matrix)))
|
|
return parents, candidates, corners, [matrix]*48, .016, tuple(range(1, 49))
|
|
|
|
|
|
def test_all_legal_candidates_use_fixed_split_and_propagate_parent_uncertainty(observer_images):
|
|
inherited = (math.radians(.05), .00005)
|
|
result = fit_observer_pose(*observer_images, inherited=inherited)
|
|
assert np.allclose(result.translation_xyz_m, [.045, .02, 0.], atol=1e-8)
|
|
assert Rotation.from_quat(result.quaternion_xyzw).magnitude() < 1e-6
|
|
assert result.rotation_uncertainty_rad >= inherited[0]
|
|
assert result.translation_uncertainty_m >= inherited[1]
|
|
assert set(result.training_stamps).isdisjoint(result.validation_stamps)
|
|
assert result.training_stamps == tuple(range(1, 49, 2))
|
|
|
|
|
|
@pytest.mark.parametrize('inherited', [(math.radians(2.274), .0005), (0., .001)])
|
|
def test_exhausted_parent_budget_is_rejected_before_fitting(observer_images, monkeypatch, inherited):
|
|
monkeypatch.setattr('linkerhand_calibration.core.geometry.tag_pose.observer_pose.least_squares',
|
|
lambda *a, **kw: pytest.fail('fitting cannot recover spent parent precision'))
|
|
with pytest.raises(ValueError, match='parent_precision_budget_exhausted'):
|
|
fit_observer_pose(*observer_images, inherited=inherited)
|
|
|
|
|
|
@pytest.mark.parametrize('fault', ['missing', 'moving', 'installation', 'unobservable', 'uncertain_parent'])
|
|
def test_invalid_observer_evidence_cannot_authorize_a_pose(observer_images, fault):
|
|
parents, candidates, corners, matrices, size, stamps = observer_images
|
|
inherited = (0., 0.)
|
|
if fault == 'missing':
|
|
parents, candidates, corners, matrices, stamps = [v[:10] for v in (parents, candidates, corners, matrices, stamps)]
|
|
elif fault in {'moving', 'installation'}:
|
|
corners = [np.array(pixels, copy=True) for pixels in corners]
|
|
for i in (range(1, len(corners), 2) if fault == 'moving' else range(len(corners)//2, len(corners))):
|
|
corners[i] += [10., -10.]
|
|
elif fault == 'unobservable':
|
|
parents = [parents[0]]*len(parents)
|
|
else:
|
|
inherited = (math.radians(2), .01)
|
|
with pytest.raises(ValueError, match='observer_'):
|
|
fit_observer_pose(parents, candidates, corners, matrices, size, stamps, inherited=inherited)
|
|
|
|
|
|
@pytest.mark.integration
|
|
def test_measured_observer_pose_resolves_only_a_separated_hinge_family(observer_images):
|
|
from linkerhand_calibration.core.geometry.tag_pose.pose_bridge import SharedTagPoseBridge
|
|
from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion
|
|
from test_motion_image_diagnostics import frames, RELATIONS
|
|
motion = frames(alternatives=True)
|
|
unresolved = resolve_image_motion(motion, RELATIONS)
|
|
assert not unresolved.resolved and unresolved.reason == 'image_motion_families_not_distinguishable'
|
|
measured = fit_observer_pose(*observer_images)
|
|
stationary = tuple(replace(motion[0], evidence=replace(motion[0].evidence,
|
|
stamp_ns=10_000_000_000+i*30_000_000)) for i in range(10))
|
|
bridge = SharedTagPoseBridge(RELATIONS[0], measured.quaternion_xyzw, measured.translation_xyz_m,
|
|
stationary, rotation_uncertainty_rad=measured.rotation_uncertainty_rad,
|
|
translation_uncertainty_m=measured.translation_uncertainty_m)
|
|
result = resolve_image_motion(motion, RELATIONS, pose_bridges=(bridge,))
|
|
assert result.resolved, result
|
|
assert all(c.status == 'consistent' for h in result.hypotheses if h.branches == result.model.branches
|
|
for c in h.pose_bridge_checks)
|
|
|
|
|
|
def test_observer_roles_do_not_change_the_current_task_or_require_visibility():
|
|
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
from linkerhand_calibration.runtime.motion_execution import MotionCommand
|
|
from linkerhand_calibration.runtime.observer_reference import observer_roles
|
|
profile = load_bundled_hand_profile('o30_right_18')
|
|
spec = profile.motion.joint_zero_references['middle_pip']
|
|
motion = MotionCommand('zero_approach', spec.command, 200., task_key=spec.task_key, reference_joints=('middle_pip',))
|
|
assert 'middle_dip' in observer_roles(profile, motion)
|
|
assert observer_roles(profile, replace(motion, phase='sweep')) == set()
|
|
assert motion.reference_joints == ('middle_pip',)
|