133 lines
6.2 KiB
Python
133 lines
6.2 KiB
Python
"""Independent pinhole projections exercise the real PnP/capture boundary."""
|
|
|
|
from dataclasses import replace
|
|
from types import SimpleNamespace
|
|
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation
|
|
import pytest
|
|
|
|
from linkerhand_calibration.profiles import load_bundled_hand_profile
|
|
from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture
|
|
from linkerhand_calibration.runtime.reference_lock import ReferenceLock
|
|
from linkerhand_calibration.runtime.motion_execution import MotionCommand
|
|
|
|
|
|
def projected_square(size, translation, rpy=(.1, -.15, .03)):
|
|
# IPPE square order: top-left, top-right, bottom-right, bottom-left.
|
|
s = size/2
|
|
points = np.array([[-s, s, 0], [s, s, 0], [s, -s, 0], [-s, -s, 0]])
|
|
xyz = points @ Rotation.from_euler("xyz", rpy).as_matrix().T + translation
|
|
return (xyz[:, :2]/xyz[:, 2:])*1200 + [640, 480]
|
|
|
|
|
|
def make_capture():
|
|
profile = load_bundled_hand_profile("l6_right_8")
|
|
front = profile.vision.views[0]
|
|
tags = tuple(replace(tag, size_m=.024 if tag.role == "thumb_pitch" else .016) for tag in front.tags)
|
|
profile = replace(profile, vision=replace(profile.vision,
|
|
views=(replace(front, tags=tags),)+profile.vision.views[1:]))
|
|
lock = ReferenceLock({v.name: next(t.tag_id for t in v.tags if t.fixed_reference) for v in profile.vision.views})
|
|
capture = ObservationCapture(profile, reference_lock=lock,
|
|
extrinsics=SimpleNamespace(transform=lambda _: np.eye(4)))
|
|
return profile, lock, capture
|
|
|
|
|
|
def frame(profile, stamp, *, omit=(), shift=0, feedback=True):
|
|
roles = {"front_base": (.0, .0, .45), "thumb_pitch": (.05, .03, .45), "thumb_dip": (.08, .05, .45)}
|
|
sizes = {tag.role: tag.size_m for tag in profile.vision.views[0].tags}
|
|
corners = {role: projected_square(sizes[role], xyz)+[shift, 0] for role, xyz in roles.items() if role not in omit}
|
|
values = tuple(map(float, profile.command.baseline_values))
|
|
return CaptureFrame("front", stamp, np.array([[1200, 0, 640], [0, 1200, 480], [0, 0, 1]]),
|
|
corners, values if feedback else None, values, command_directions=("increasing",)*len(values))
|
|
|
|
|
|
def test_only_ten_unique_post_start_valid_frames_lock_and_mounts_keep_per_tag_size():
|
|
profile, lock, capture = make_capture()
|
|
for index in range(20):
|
|
capture.consume(frame(profile, index+1))
|
|
assert not lock.references # Preview is not a reference.
|
|
lock.begin_session()
|
|
capture.reset()
|
|
lock.start_locking()
|
|
for _ in range(20):
|
|
capture.consume(frame(profile, 100), collect_installations=True)
|
|
assert not lock.references
|
|
for index in range(9):
|
|
capture.consume(frame(profile, 101+index), collect_installations=True)
|
|
fixed, moving = capture.fingerprint_poses()
|
|
assert "front" in fixed
|
|
np.testing.assert_allclose(fixed["front"]["translation_xyz_m"], [0, 0, .45], atol=1e-7)
|
|
assert "front:thumb_pitch" in moving
|
|
assert len(capture.installations["front:thumb_pitch"]) == 10
|
|
|
|
|
|
def test_short_visual_loss_discards_frames_cached_base_is_not_fake_image_data():
|
|
profile, lock, capture = make_capture()
|
|
lock.begin_session()
|
|
lock.start_locking()
|
|
for i in range(10):
|
|
capture.consume(frame(profile, i+1), collect_installations=True)
|
|
motion = MotionCommand("sweep", tuple(profile.command.baseline_values), 1,
|
|
"thumb_pitch_dip_front", 0, 0, "decreasing")
|
|
rows, moved = capture.consume(frame(profile, 20, omit=("front_base",)), motion)
|
|
assert moved is None and len(rows) == 2
|
|
assert rows[0]["locked_reference_roles"] == ["front_base"]
|
|
assert "image_relative_xy_px" not in rows[0]
|
|
assert capture.consume(frame(profile, 21, feedback=False), motion)[0] == ()
|
|
assert capture.consume(frame(profile, 22, omit=("thumb_pitch",)), motion)[0] == ()
|
|
assert len(capture.consume(frame(profile, 23), motion)[0]) == 2
|
|
assert len(capture.installations["front:thumb_pitch"]) == 10 # Frozen during motion.
|
|
|
|
|
|
@pytest.mark.parametrize("drift", [6, 200])
|
|
def test_fixed_reference_displacement_requires_ten_confirming_images(drift):
|
|
profile, lock, capture = make_capture()
|
|
lock.begin_session()
|
|
lock.start_locking()
|
|
for i in range(10):
|
|
capture.consume(frame(profile, i+1))
|
|
for i in range(9):
|
|
assert capture.consume(frame(profile, 20+i, shift=drift))[1] is None
|
|
assert capture.consume(frame(profile, 29, shift=drift))[1] is not None
|
|
|
|
|
|
def test_joint_zero_only_requires_its_own_parent_child_tags():
|
|
from linkerhand_calibration.core.domain.reference import build_joint_zero_reference
|
|
profile, lock, capture = make_capture()
|
|
lock.begin_session()
|
|
lock.start_locking()
|
|
for i in range(10):
|
|
capture.consume(frame(profile, i+1))
|
|
name = "rh_thumb_cmc_pitch"
|
|
spec = profile.motion.joint_zero_references[name]
|
|
motion = MotionCommand("joint_zero", spec.command, 1, task_key=spec.task_key,
|
|
command_index=0, cycle=0, zero_joints=(name,))
|
|
rows = []
|
|
for i in range(10):
|
|
samples, movement = capture.consume(frame(profile, 20+i, omit=("thumb_dip",)), motion)
|
|
assert movement is None
|
|
rows.extend(samples)
|
|
assert len(rows) == 10 and all(row["kind"] == "joint_zero_sample" for row in rows)
|
|
reference = build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=2)
|
|
assert reference.joint == name and len(reference.samples) == 10
|
|
|
|
|
|
def test_articulated_capture_discards_partial_moving_tags_and_records_cached_base():
|
|
profile, lock, capture = make_capture()
|
|
profile = replace(profile, measurement=replace(profile.measurement,
|
|
candidate_selection_tasks=frozenset({"thumb_pitch_dip_front"})))
|
|
capture.profile = profile
|
|
lock.begin_session()
|
|
lock.start_locking()
|
|
for i in range(10):
|
|
capture.consume(frame(profile, i+1))
|
|
motion = MotionCommand("sweep", profile.command.baseline_values, 1,
|
|
"thumb_pitch_dip_front", 0, 0, "decreasing")
|
|
assert capture.consume(frame(profile, 20, omit=("thumb_dip",)), motion)[0] == ()
|
|
samples, _ = capture.consume(frame(profile, 21, omit=("front_base",)), motion)
|
|
candidate = next(r for r in samples if r["kind"] == "pnp_candidate_frame")
|
|
assert candidate["roles"]["front_base"]["locked_reference"] is True
|
|
assert candidate["roles"]["front_base"]["corners_xy"] is None
|
|
assert candidate["roles"]["front_base"]["selected"]
|